Crash detection

edit

Hi,

I try to code a crash detection system with a publisher and a subscriber. Now, when i shutdown (^C) the publisher, the subscriber is in a state of "wait". It waits until it sees another msg on the topic and won't execute my code after the subscribe function.

I've tried to detect a decrease of publisher on the topic but it doesn't work.

Can you help me with this ?

Here are my code :

My Primary (Publisher) :

int main (int argc, char **argv){

int i=0;
ros::init(argc, argv, "primary");
ros::NodeHandle primary;
ros::Publisher pub_wd;
pub_wd = primary.advertise<primary::Test>("primwd", 1000);
while(ros::ok())
{

    i++;
    ::primary::Test test_msg;
    test_msg.count = i;
    if(i == 16001){
            i = 1;
    }
    pub_wd.publish(test_msg);
    cout << "Le message publié est " << i << endl;
    usleep(100000);
}
return 0;
}

My watchdog (Subscriber) :

watchdog::watchdog() {

    i = 0;
    inc = 1;
    etat_prec = 0;
    pub=1;

}

int watchdog::check_watchdog(){

    if(i!=2){
        if(inc==etat_prec){
            cout << "J'entre dans la boucle inc = etat prec" << endl;
            i++;
            return 0;
        }
        else if(inc!=etat_prec){
            cout << "J'entre dans la boucle inc != etat prec" << endl;
            i = 0;
            etat_prec = inc;
            return 0;
        }
    }
    else if(i==2){
        cout << "le primaire a crashé" << endl;
        usleep(100000);
        return 1;
    }
}

void watchdog::callback (const primary::Test::ConstPtr& test_msg){

    inc = test_msg->count;
    pub = sub.getNumPublishers();
    cout << "Le compteur vaut : " << inc << endl;
    cout << "Nombre de publisher : " << pub << endl;
    check_watchdog();

}

void watchdog::subscri(){

    sub = n.subscribe("primwd", 1000, &watchdog::callback,this);
    if(pub==1){
    ros::spin();
    }
    else{
    check_watchdog();
    }
}

void watchdog::mode_fonct(){

    subscri();
}

And the main of my WD :

int main (int argc, char **argv) {
    ros::init(argc, argv, "watchdog"); // Initialisation de ROS
    int WD;
    watchdog wd; // Création d'un watchdog

    while(ros::ok()){

        wd.mode_fonct();

    }
    cout << "Fin d'execution" << endl;

    return 0;
}

My WD and my Primary are in different packages and i have a wd.h as well but there is no problems with it :

class watchdog {
    int etat_prec,inc,i,pub;
    ros::NodeHandle n; // Création du noeud Watchdog
    ros::Subscriber sub; 
public:
    watchdog();
    int check_watchdog();
    void callback (const primary::Test::ConstPtr&);
    void mode_fonct();
    void subscri();
};