Component.h

Go to the documentation of this file.

#ifndef __COMPONENT_H
        #define __COMPONENT_H

#include <ros/ros.h>
#include <pthread.h>
#include <string>
#include <vector>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define DEFAULT_THREAD_DESIRED_HZ       40.0
#define DEFAULT_THREAD_PRIORITY     25

#define USEC_PER_SEC                    1000000
#define NSEC_PER_SEC                    1000000000
#define Pi                      3.141592654

enum ReturnValue{
        OK = 0,
        INITIALIZED,
        THREAD_RUNNING,
        ERROR = -1,
        NOT_INITIALIZED = -2,
        THREAD_NOT_RUNNING = -3,
        COM_ERROR = -4,
        NOT_ERROR = -5
};
enum States{
        INIT_STATE,
        STANDBY_STATE,
        READY_STATE,
        EMERGENCY_STATE,
        FAILURE_STATE,
        SHUTDOWN_STATE,
        UNKNOWN_STATE
};

extern int clock_nanosleep(clockid_t __clock_id, int __flags,
                        __const struct timespec *__req,
                        struct timespec *__rem);

static inline void tsnorm(struct timespec *ts)
{
        while (ts->tv_nsec >= NSEC_PER_SEC) {
                ts->tv_nsec -= NSEC_PER_SEC;
                ts->tv_sec++;
        }
}
static inline void radnorm(double* radians)
{
        while (*radians >= (Pi)) {
                *radians -= 2.0 * Pi;
                }
        while (*radians <= (-Pi)) {
                *radians += 2.0 * Pi;
                }
}
static inline void radnorm2(double* radians)
{
        while (*radians >= (Pi)) {
                *radians -= 2.0 * Pi;
                }
        while (*radians <= (-Pi)) {
                *radians += 2.0 * Pi;
                }
}
static inline long calcdiff(struct timespec t1, struct timespec t2)
{
        long diff;
        diff = USEC_PER_SEC * ((int) t1.tv_sec - (int) t2.tv_sec);
        diff += ((int) t1.tv_nsec - (int) t2.tv_nsec) / 1000;
        return diff;
}
struct thread_param{
        int prio;                       // Thread priority level 0[min]-80[max]
        int clock;              // CLOCK_MONOTONIC or CLOCK_REALTIME
};
typedef struct thread_data{
    struct thread_param pthreadPar;
    double dDesiredHz;
    double dRealHz;
    pthread_t pthreadId;
}thread_data;

void *AuxControlThread(void *threadParam);

using namespace std;

class Component{
    friend void *AuxControlThread(void *threadParam);
        protected:
                bool bInitialized;
                bool bRunning;
                thread_data threadData;
                vector<thread_data> vThreadData;
                States iState;
                States iOldState;
                string sComponentName;
    private:
        pthread_mutex_t mutexThread;
        public:
                Component(double desired_hz);
                virtual ~Component();
                virtual ReturnValue Setup();
                virtual ReturnValue ShutDown();
                virtual ReturnValue Start();
                virtual ReturnValue Stop();
                States GetState();
                virtual char *GetStateString();
                char *GetStateString(States state);
                double GetUpdateRate();
                
        protected:
                virtual void ControlThread();
                virtual ReturnValue Open();
                virtual ReturnValue Close();
                virtual ReturnValue Allocate();
                virtual ReturnValue Free();
                virtual ReturnValue Configure();
                virtual void InitState();
                virtual void StandbyState();
                virtual void ReadyState();
                virtual void EmergencyState();
                virtual void FailureState();
                virtual void AllState();
                virtual void SwitchToState(States new_state);
                ReturnValue GetThreadData(thread_data *data);
    public:
                ReturnValue CreateTask(int prio, double frec, void *(*start_routine)(void*));


};

#endif