Component.cc

Go to the documentation of this file.

#include <pthread.h>
#include <purepursuit_planner/Component.h>


void *AuxControlThread(void *threadParam){
        Component *ComponentThread = (Component *)threadParam;
        ComponentThread->ControlThread();
    pthread_detach(pthread_self());
        return NULL;
}

Component::Component(double desired_hz){
        // Set main flags to false
        bInitialized = bRunning = false;
        threadData.dRealHz = 0.0;
        if(desired_hz <= 0.0)
                desired_hz = DEFAULT_THREAD_DESIRED_HZ;
        threadData.dDesiredHz = desired_hz;
        iState = SHUTDOWN_STATE;
        // Realizar para cada una de las clases derivadas
        sComponentName.assign("Component");
        
        // mutex intitialization
        pthread_mutex_init(&mutexThread, NULL);
        // Real-Time parameters
        threadData.pthreadPar.prio = 25;                                // Priority level 0[min]-80[max]
        threadData.pthreadPar.clock= CLOCK_REALTIME;    // 0-CLOCK_MONOTONIC 1-CLOCK_REALTIME
}

Component::~Component(){
        
        pthread_mutex_destroy(&mutexThread);
}

ReturnValue Component::Setup(){
        // Checks if has been initialized
        if(bInitialized){
                ROS_INFO("%s::Setup: Already initialized",sComponentName.c_str());
                
                return INITIALIZED;
        }
        
        //
        // Setups another subcomponents if it's necessary //

        // Allocates memory
        if(Allocate() != OK){
                ROS_ERROR("%s::Setup: Error in Allocate",sComponentName.c_str());
                return ERROR;
        }
        // Opens devices used by the component
        if(Open() != OK){
                ROS_ERROR("%s::Setup: Error in Open",sComponentName.c_str());
                
                return ERROR;
        }       
        //
        // Configure the component
        if(Configure() != OK){
                ROS_ERROR("%s::Setup: Error in Configure",sComponentName.c_str());
                return ERROR;
        }

        bInitialized = true;

        return OK;
}

ReturnValue Component::ShutDown(){
        
        if(bRunning){
                ROS_INFO("%s::ShutDown: Impossible while thread running, first must be stopped",sComponentName.c_str());
                return THREAD_RUNNING;
        }
        if(!bInitialized){
                ROS_INFO("%s::ShutDown: Impossible because of it's not initialized", sComponentName.c_str());
                return NOT_INITIALIZED;
        }

        //
        // ShutDowns another subcomponents if it's necessary //
                
        if(Close() != OK){
                ROS_ERROR("%s::ShutDown: Error Closing", sComponentName.c_str());
                return ERROR;
        }
        if(Free() != OK){
                ROS_ERROR("%s::ShutDown: Error in Free", sComponentName.c_str());
                return ERROR;
        }

        bInitialized = false;

        return OK;
}

ReturnValue Component::Configure(){

        return OK;
}

ReturnValue Component::Allocate(){

        return OK;
}

ReturnValue Component::Free(){

        return OK;
}

ReturnValue Component::Open(){

        return OK;
}

ReturnValue Component::Close(){

        return OK;
}

ReturnValue Component::Start(){
        
        pthread_attr_t attr;    // Thread attributed for the component threads spawned in this function

        // If the component is not initialized, we can't start
        if(!bInitialized){
                ROS_INFO("%s::Start: the component is not initialized", sComponentName.c_str());
                return NOT_INITIALIZED;
        }
        //
        // Starts another subcomponents if it's necessary //
        //
        if(!bRunning) {
                ROS_INFO("%s::Start: launching the thread", sComponentName.c_str());
                

                pthread_attr_init(&attr);
                pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
                bRunning = true;
                // Creation of the main thread
                if(pthread_create(&threadData.pthreadId, &attr, AuxControlThread, this) != 0)   {
                        ROS_ERROR("%s::Start: Could not create ControlThread", sComponentName.c_str());
                        
                        pthread_attr_destroy(&attr);
                        bRunning = false;
                        return ERROR;
                }

                pthread_attr_destroy(&attr);
                return OK;

        }else {
                ROS_INFO("%s::Start: the component's thread is already running", sComponentName.c_str());
                return THREAD_RUNNING;
        }

}

ReturnValue Component::Stop(){
        
        if(!bRunning){
                ROS_INFO("%s::Stop: Thread not running", sComponentName.c_str());
        
                return THREAD_NOT_RUNNING;
        }
        //
        // Stops another subcomponents, if it's necessary //
        //
        ROS_INFO("%s::Stop: Stopping the thread", sComponentName.c_str());
        
        bRunning = false;

        usleep(100000);

        return OK;
}

void Component::ControlThread(){
        struct sched_param schedp;
        int policy = threadData.pthreadPar.prio? SCHED_FIFO : SCHED_OTHER;
        struct timespec now, next, interval;
        struct timespec last_time;  // Necesarry to calculates real frec
        long diff;
        unsigned long sampling_period_us;
        
        // Robotnik thread priority and timing management
        memset(&schedp, 0, sizeof(schedp));
        schedp.sched_priority = threadData.pthreadPar.prio;
        sched_setscheduler(0, policy, &schedp);
        clock_gettime(threadData.pthreadPar.clock, &now);
        last_time = next = now;
        next.tv_sec++;  // start in next second?
        sampling_period_us =  (long unsigned int) (1.0 / threadData.dDesiredHz * 1000000.0);
        interval.tv_sec = sampling_period_us / USEC_PER_SEC;  // interval parameter in uS
        interval.tv_nsec = (sampling_period_us % USEC_PER_SEC)*1000;

        //sprintf(cAux, "%s::ControlThread: Starting the thread", sComponentName.c_str());
        //rlcLog->AddEvent((char *)cAux);
        // Begin thread execution on init state
        SwitchToState(INIT_STATE);

        while(bRunning) { // Executes state machine code while bRunning flag is active
                clock_nanosleep(threadData.pthreadPar.clock, TIMER_ABSTIME, &next, NULL);
                clock_gettime(threadData.pthreadPar.clock, &now);
                next.tv_sec += interval.tv_sec;
                next.tv_nsec += interval.tv_nsec;
                tsnorm(&next);

                diff = calcdiff(now, last_time);
                last_time = now;
                // Thread frequency update
                threadData.dRealHz = 1.0/(diff / 1000000.0); // Compute the update rate of this thread
                // Test: controlamos que la frecuencia real no difiera de la teórica
                if(threadData.dRealHz < (1.0/5.0)*threadData.dDesiredHz){
                        ROS_ERROR("%s:ControlThread: Delay on control loop. Desired Hz is %lf, but real is %lf", sComponentName.c_str(), threadData.dDesiredHz, threadData.dRealHz);
                        
                }

                switch (iState){
                        case INIT_STATE:
                                InitState();
                        break;
                        case STANDBY_STATE:
                                StandbyState();
                        break;
                        case READY_STATE:
                                ReadyState();
                        break;
                        case EMERGENCY_STATE:
                                EmergencyState();
                        break;
                        case FAILURE_STATE:
                                FailureState();
                        break;
                        default:
                        break;
                }
                AllState();
        }
        SwitchToState(SHUTDOWN_STATE);
        
        ROS_INFO("%s:ControlThread: Exiting main Thread", sComponentName.c_str());
        
        usleep(100000); // Sleep for 100 milliseconds and then exit

}

void Component::InitState(){

}

void Component::StandbyState(){

}

void Component::ReadyState(){

}

void Component::EmergencyState(){

}

void Component::FailureState(){

}

void Component::AllState(){

}

double Component::GetUpdateRate(){
        return threadData.dRealHz;
}

States Component::GetState(){
        return iState;
}

char *Component::GetStateString(){
        switch(iState){
                case INIT_STATE:
                        return (char *)"INIT";
                break;
                case STANDBY_STATE:
                        return (char *)"STANDBY";
                break;
                case READY_STATE:
                        return (char *)"READY";
                break;
                case EMERGENCY_STATE:
                        return (char *)"EMERGENCY";
                break;
                case FAILURE_STATE:
                        return (char *)"FAILURE";
                break;
                case SHUTDOWN_STATE:
                        return (char *)"SHUTDOWN";
                break;
                default:
                        return (char *)"UNKNOWN";
                break;
        }
}

char *Component::GetStateString(States state){
        switch(state){
                case INIT_STATE:
                        return (char *)"INIT";
                break;
                case STANDBY_STATE:
                        return (char *)"STANDBY";
                break;
                case READY_STATE:
                        return (char *)"READY";
                break;
                case EMERGENCY_STATE:
                        return (char *)"EMERGENCY";
                break;
                case FAILURE_STATE:
                        return (char *)"FAILURE";
                break;
                case SHUTDOWN_STATE:
                        return (char *)"SHUTDOWN";
                break;
                default:
                        return (char *)"UNKNOWN";
                break;
        }
}


void Component::SwitchToState(States new_state){
        
        if(new_state == iState)
                return;


        ROS_INFO("%s::SwitchToState: %s -> %s", sComponentName.c_str(), GetStateString(iState), GetStateString(new_state));     
        iState = new_state;

}

ReturnValue Component::GetThreadData(thread_data *data){
    pthread_t auxId = pthread_self();
    ReturnValue ret = ERROR;
    // Main thread
    if(auxId == threadData.pthreadId){
        *data = threadData;
        ROS_INFO("Component::GetThreadData: Main thread");
        return OK;
    }
    // Search the Id in the list
    pthread_mutex_lock(&mutexThread);
        for(unsigned int i = 0; i < vThreadData.size(); i++){
            
            if(auxId == vThreadData[i].pthreadId){  // Encontrado
                *data = vThreadData[i];
                ret = OK;
                break;
            }
        }
    pthread_mutex_unlock(&mutexThread);

    return ret;
}

ReturnValue Component::CreateTask(int prio, double frec, void *(*start_routine)(void*)){
        pthread_attr_t attr;    // Thread attributed for the component threads spawned in this function
    thread_data new_thread;
    if(frec <= 0.0)
        frec = DEFAULT_THREAD_DESIRED_HZ;
    if(prio < 0)
        prio = DEFAULT_THREAD_PRIORITY;
    //
        // If the component is not initialized, we can't start
        if(!bRunning){
                ROS_INFO("%s::CreateTask: the component is not running", sComponentName.c_str());
                return THREAD_NOT_RUNNING;
        }

    pthread_attr_init(&attr);
    pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
    // Protect the creation and modification of threads with a mutex
    pthread_mutex_lock(&mutexThread);

        // Creation of the main thread
        if(pthread_create(&new_thread.pthreadId, &attr, start_routine, this) != 0)      {
            ROS_ERROR("%s::CreateTask: Could not create new thread", sComponentName.c_str());
            pthread_attr_destroy(&attr);
            pthread_mutex_unlock(&mutexThread);
            return ERROR;
        }
        pthread_attr_destroy(&attr);
        new_thread.dDesiredHz = frec;
        new_thread.dRealHz = 0.0;
        new_thread.pthreadPar.prio = prio;
        new_thread.pthreadPar.clock = CLOCK_REALTIME;
        // Adds information for the new thread
        vThreadData.push_back(new_thread);
        
    pthread_mutex_unlock(&mutexThread);

    return OK;
}