private and task changes

software/raspberry/superviseur-robot/lib/comrobot.cpp

@@ -131,7 +131,11 @@ int ComRobot::Open(string usart) {
  * @return Success if above 0, failure if below 0
  */
 int ComRobot::Close() {
+#ifdef __SIMULATION__
+    return close(sock);
+#elif
     return close(fd);
+#endif
 }
 
 /**
@@ -155,17 +159,22 @@ Message *ComRobot::Write(Message* msg) {
 
         char buffer[1024] = {0};
         cout << "[" << __PRETTY_FUNCTION__ << "] Send command: " << s << endl << flush;
-        send(sock, s.c_str(), s.length(), 0);
+        send(sock, s.c_str(), s.length(), MSG_NOSIGNAL);
 
         int valread = read(sock, buffer, 1024);
-        if (valread < 0) {
+
+        if (valread == 0) {
+            cout << "The communication is out of order" << endl;
+            msgAnswer = new Message(MESSAGE_ANSWER_COM_ERROR);
+        } else if (valread < 0) {
+            cout << "Timeout" << endl;
             msgAnswer = new Message(MESSAGE_ANSWER_ROBOT_TIMEOUT);
         } else {
             string s(&buffer[0], valread);
             msgAnswer = StringToMessage(s);
-            //msgAnswer = new Message(MESSAGE_ANSWER_ACK);
+            cout << "Response: " << buffer << ", id: " << msgAnswer->GetID() << endl;
         }
-        cout << "response: " <<  buffer << ", id: " << msgAnswer->GetID() << endl;
+
 #else       
         AddChecksum(s);
 

software/raspberry/superviseur-robot/tasks.cpp

@@ -19,17 +19,17 @@
 #include <stdexcept>
 
 // Déclaration des priorités des taches
-#define PRIORITY_TSERVER 30
+#define PRIORITY_TSERVER 10
-#define PRIORITY_TOPENCOMROBOT 20
+#define PRIORITY_TOPENCOMROBOT 50
 #define PRIORITY_TMOVE 20
 #define PRIORITY_TSENDTOMON 22
-#define PRIORITY_TRECEIVEFROMMON 25
+#define PRIORITY_TRECEIVEFROMMON 50
 #define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 22
 #define PRIORITY_TSTARTROBOTWITHWATCHDOG 22
 #define PRIORITY_TCAMERA 21
+#define PRIORITY_DETECTLOSTSUPROB 21
 #define PRIORITY_TVISION 26 
 
-
 /*
  * Some remarks:
  * 1- This program is mostly a template. It shows you how to create tasks, semaphore
@@ -146,21 +146,21 @@ void Tasks::Init() {
         cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
-    if (err = rt_sem_create(&sem_askBattery, NULL, 0, S_FIFO)) {
+        
-        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
+     if (err = rt_sem_create(&sem_detectLostSupRob, NULL, 0, S_FIFO)) {
-        exit(EXIT_FAILURE);
-    }
-    if (err = rt_sem_create(&sem_allowStartReceive, NULL, 0, S_FIFO)) {
-        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
-        exit(EXIT_FAILURE);
-    }   
-    if (err = rt_sem_create(&sem_allowStartCapture, NULL, 0, S_FIFO)) {
         cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
 
-    //Initialization for some specific sem: Allow to run the first time
+    if (err = rt_sem_create(&sem_askBattery, NULL, 0, S_FIFO)) {
-    rt_sem_v(&sem_allowStartReceive);
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    
+     if (err = rt_sem_create(&sem_restart, NULL, 0, S_FIFO)) {
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    } 
     
     cout << "Semaphores created successfully" << endl << flush;
 
@@ -168,31 +168,36 @@ void Tasks::Init() {
     /* Tasks creation                                                                     */
     /**************************************************************************************/
     if (err = rt_task_create(&th_server, "th_server", 0, PRIORITY_TSERVER, 0)) {
-        cerr << "Error task create: " << strerror(-err) << endl << flush;
+        cerr << "Error task create server: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
     if (err = rt_task_create(&th_sendToMon, "th_sendToMon", 0, PRIORITY_TSENDTOMON, 0)) {
-        cerr << "Error task create: " << strerror(-err) << endl << flush;
+        cerr << "Error task create sendtoMon: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
     if (err = rt_task_create(&th_receiveFromMon, "th_receiveFromMon", 0, PRIORITY_TRECEIVEFROMMON, 0)) {
-        cerr << "Error task create: " << strerror(-err) << endl << flush;
+        cerr << "Error task create receiveFromMon: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
     if (err = rt_task_create(&th_openComRobot, "th_openComRobot", 0, PRIORITY_TOPENCOMROBOT, 0)) {
-        cerr << "Error task create: " << strerror(-err) << endl << flush;
+        cerr << "Error task create openComRobot: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
     if (err = rt_task_create(&th_startRobotWithoutWatchdog, "th_startRobotWithoutWatchdog", 0, PRIORITY_TSTARTROBOTWITHOUTWATCHDOG, 0)) {
-        cerr << "Error task create: " << strerror(-err) << endl << flush;
+        cerr << "Error task create startRobotWithoutWatchdog: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
     if (err = rt_task_create(&th_startRobotWithWatchdog, "th_startRobotWithWatchdog", 0, PRIORITY_TSTARTROBOTWITHWATCHDOG, 0)) {
-        cerr << "Error task create: " << strerror(-err) << endl << flush;
+        cerr << "Error task create startRobotWithWatchdog: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
     if (err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0)) {
-        cerr << "Error task create: " << strerror(-err) << endl << flush;
+        cerr << "Error task create move: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    
+    if (err = rt_task_create(&th_detectLostSupRob, "th_detectLostSupRob", 0, PRIORITY_DETECTLOSTSUPROB, 0)) {
+        cerr << "Error task create detectLostSupRob: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
     if (err = rt_task_create(&th_askBattery, "th_askBattery", 0, PRIORITY_TMOVE, 0)) {
@@ -220,37 +225,46 @@ void Tasks::Init() {
  * @brief Démarrage des tâches
  */
 void Tasks::Run() {
+    
+        cout << "Coucou" << endl << flush;
+
         rt_task_set_priority(NULL, T_LOPRIO);
         int err;
 
         if (err = rt_task_start(&th_server, (void(*)(void*)) & Tasks::ServerTask, this)) {
-        cerr << "Error task start: " << strerror(-err) << endl << flush;
+            cerr << "Error task start server: " << strerror(-err) << endl << flush;
             exit(EXIT_FAILURE);
         }
         if (err = rt_task_start(&th_sendToMon, (void(*)(void*)) & Tasks::SendToMonTask, this)) {
-        cerr << "Error task start: " << strerror(-err) << endl << flush;
+            cerr << "Error task start sendToMon: " << strerror(-err) << endl << flush;
             exit(EXIT_FAILURE);
         }
         if (err = rt_task_start(&th_receiveFromMon, (void(*)(void*)) & Tasks::ReceiveFromMonTask, this)) {
-        cerr << "Error task start: " << strerror(-err) << endl << flush;
+            cerr << "Error task start receiveFromMon: " << strerror(-err) << endl << flush;
             exit(EXIT_FAILURE);
         }
         if (err = rt_task_start(&th_openComRobot, (void(*)(void*)) & Tasks::OpenComRobot, this)) {
-        cerr << "Error task start: " << strerror(-err) << endl << flush;
+            cerr << "Error task start openComRobot: " << strerror(-err) << endl << flush;
             exit(EXIT_FAILURE);
         }
         if (err = rt_task_start(&th_startRobotWithoutWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithoutWatchdog, this)) {
-        cerr << "Error task start: " << strerror(-err) << endl << flush;
+            cerr << "Error task start startRobotWithoutWatchdog: " << strerror(-err) << endl << flush;
             exit(EXIT_FAILURE);
         }
         if (err = rt_task_start(&th_startRobotWithWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithWatchdog, this)) {
-        cerr << "Error task start: " << strerror(-err) << endl << flush;
+            cerr << "Error task start startRobotWithWatchdog: " << strerror(-err) << endl << flush;
             exit(EXIT_FAILURE);
         }
         if (err = rt_task_start(&th_move, (void(*)(void*)) & Tasks::MoveTask, this)) {
-        cerr << "Error task start: " << strerror(-err) << endl << flush;
+            cerr << "Error task start move: " << strerror(-err) << endl << flush;
             exit(EXIT_FAILURE);
         }
+
+        if (err = rt_task_start(&th_detectLostSupRob, (void(*)(void*)) & Tasks::DetectLostSupRob, this)) {
+            cerr << "Error task start detectLostSupRob: " << strerror(-err) << endl << flush;
+            exit(EXIT_FAILURE);
+        }    
+
         if (err = rt_task_start(&th_askBattery, (void(*)(void*)) & Tasks::AskBattery, this)) {
             cerr << "Error task start: " << strerror(-err) << endl << flush;
             exit(EXIT_FAILURE);
@@ -275,6 +289,10 @@ void Tasks::Stop() {
 void Tasks::Join() {
     cout << "Tasks synchronized" << endl << flush;
     rt_sem_broadcast(&sem_barrier);
+        
+    //Initialization for some specific sem: Allow to run the first time
+    //rt_sem_broadcast(&sem_restart);
+    
     pause();
 }
 
@@ -285,6 +303,8 @@ void Tasks::ServerTask(void *arg) {
     int status;
     
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
+    
+    while(1){
         // Synchronization barrier (waiting that all tasks are started)
         rt_sem_p(&sem_barrier, TM_INFINITE);
 
@@ -304,29 +324,48 @@ void Tasks::ServerTask(void *arg) {
         cout << "Rock'n'Roll baby, client accepted!" << endl << flush;
         rt_sem_broadcast(&sem_serverOk);
     }
+}
 
 /**
  * @brief Thread sending data to monitor.
  */
 void Tasks::SendToMonTask(void* arg) {
     Message *msg;
+    bool kill_sendToMonOk=0;
     
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
+    
+    while(1){
         // Synchronization barrier (waiting that all tasks are starting)
         rt_sem_p(&sem_barrier, TM_INFINITE);
     
+
         /**************************************************************************************/
         /* The task sendToMon starts here                                                     */
         /**************************************************************************************/
         rt_sem_p(&sem_serverOk, TM_INFINITE);
         
-    while (1) {
+        //Initialize the loop condition
+        kill_sendToMonOk=0;
+        rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE);
+            killSendMon=0; // The message is deleted with the Write
+        rt_mutex_release(&mutex_killSendMon);
+
+        while (!kill_sendToMonOk) {
             cout << "wait msg to send" << endl << flush;
             msg = ReadInQueue(&q_messageToMon);
             cout << "Send msg to mon: " << msg->ToString() << endl << flush;
             rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
             monitor.Write(msg); // The message is deleted with the Write
             rt_mutex_release(&mutex_monitor);
+
+            rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE);
+            kill_sendToMonOk=killSendMon; // The message is deleted with the Write
+            rt_mutex_release(&mutex_killSendMon);        
+
+        }
+        
+        cout << "SendToMon Task dies" << endl << flush;
     }
 }
 
@@ -336,15 +375,17 @@ void Tasks::SendToMonTask(void* arg) {
 void Tasks::ReceiveFromMonTask(void *arg) {
     Message *msgRcv;
     bool killReceiveFromMonOk=0;
+    int status;
     
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
+        
     // Synchronization barrier (waiting that all tasks are starting)
     rt_sem_p(&sem_barrier, TM_INFINITE);
     
-    //Wait twin task to die if not first round
+    while(1){
-    rt_sem_p(&sem_allowStartReceive, TM_INFINITE);
 
         //Reinitialize control boolean
+        killReceiveFromMonOk = 0;
         rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
             killReceiveFromMon = 0;
         rt_mutex_release(&mutex_killReceiveFromMon);
@@ -354,14 +395,13 @@ void Tasks::ReceiveFromMonTask(void *arg) {
         /**************************************************************************************/
         rt_sem_p(&sem_serverOk, TM_INFINITE);
         cout << "Received message from monitor activated" << endl << flush;
+
         while (!killReceiveFromMonOk) {
             msgRcv = monitor.Read();
             cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
 
             if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
-            delete(msgRcv);
                 cout << "Connection to monitor lost" << endl;
-            monitor.Close();
 
                 rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
                     killReceiveFromMon=1;
@@ -374,30 +414,74 @@ void Tasks::ReceiveFromMonTask(void *arg) {
                 rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE);
                     killSendMon=1;
                 rt_mutex_release(&mutex_killSendMon); 
+                //Write fake message in queue to unblock Read function
+                WriteInQueue(&q_messageToMon, new Message(MESSAGE_EMPTY));
 
                 rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
                     killBattery=1;
                 rt_mutex_release(&mutex_killBattery);
 
                 rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
-                killDetectlostSupRob=1;
+                    killDetectLostSupRob=1;
                 rt_mutex_release(&mutex_killDetectLostSupRob);
 
                 rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
                     acquireImage=0;
                 rt_mutex_release(&mutex_acquireImage);
                 
-            //exit(-1);
+                rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
+                    robotStarted=0;
+                rt_mutex_release(&mutex_robotStarted);
+
+                //Wait every task to die
+                sleep(1);
+  
+                rt_mutex_acquire(&mutex_robot, TM_INFINITE);
+                    status=robot.Close();
+                rt_mutex_release(&mutex_robot);
+                
+                if(status<0){
+                    cout << "Close Robot Fail" << endl << flush;
+                }
+                else{
+                    cout << "Close Robot Success" << endl << flush;
+                }
+
+                //All closes
+                rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
+                    monitor.Close();
+                rt_mutex_release(&mutex_monitor);
+                
+                //Release restarted tasks
+                rt_sem_broadcast(&sem_barrier);             
 
 
             } else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
                 cout << "Command Open Camera Received" << endl << flush;
+
+
                 //start task Vision
 
+            } else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) {
+                cout << "Command Close Camera Received" << endl << flush;
+
+                //Trigger killVision
+                rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
+                    killVision=1;
+                rt_mutex_release(&mutex_killVision);
+
             } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
                 cout << "Command Open Communication with Robot Received" << endl << flush;
                 rt_sem_v(&sem_openComRobot);
 
+            } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_CLOSE)) {
+                cout << "Command Close Communication with Robot Received" << endl << flush;
+              
+                rt_mutex_acquire(&mutex_robot, TM_INFINITE);
+                    status=robot.Close();
+                rt_mutex_release(&mutex_robot);
+                
+
             } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
                 cout << "Command Start Robot without Watchdog Received" << endl << flush;
                 rt_sem_v(&sem_startRobotWithoutWatchdog);
@@ -413,7 +497,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
                 rt_mutex_release(&mutex_searchArena);
 
             } else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) {
-            cout << "Command Get Root Position Received" << endl << flush;
+                cout << "Command Get Robot Position Received" << endl << flush;
                 rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
                     getPosition=1;
                 rt_mutex_release(&mutex_getPosition);            
@@ -447,6 +531,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
                 rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
                     acquireImage=1;
                 rt_mutex_release(&mutex_acquireImage);
+
             } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
                     msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
                     msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
@@ -460,13 +545,14 @@ void Tasks::ReceiveFromMonTask(void *arg) {
             delete(msgRcv); // must be deleted manually, no consumer
 
             //Update loop condition
+
             rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
                 killReceiveFromMonOk = killReceiveFromMon;
             rt_mutex_release(&mutex_killReceiveFromMon);
-        
         }
 
-    rt_sem_v(&sem_allowStartReceive);
+        cout << "ReceiveFromMon dies" << endl << flush;
+    }
 }
 
 
@@ -475,9 +561,10 @@ void Tasks::ReceiveFromMonTask(void *arg) {
 /**
  * @brief Thread opening communication with the robot.
  */
-void Tasks::OpenComRobot(void *arg) {
+void Tasks::OpenComRobot(void *arg) { //PAS DE SOUCIS AU REDEMARAGE
     int status;
     int err;
+    //bool killOpenComRobotOk = 0;
 
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
     // Synchronization barrier (waiting that all tasks are starting)
@@ -502,6 +589,10 @@ void Tasks::OpenComRobot(void *arg) {
                 msgSend = new Message(MESSAGE_ANSWER_ACK);
             }
             WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
+            
+            //Trigger Detection of Communication Loss with Robot
+            rt_sem_v(&sem_detectLostSupRob);
+
         }
 }
 
@@ -516,14 +607,16 @@ void Tasks::OpenComRobot(void *arg) {
  * @brief Thread starting the communication with the robot.
  */
 void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
-    cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
+
-    // Synchronization barrier (waiting that all tasks are starting)
-    rt_sem_p(&sem_barrier, TM_INFINITE);
     int killBatteryOk=0;
     Message * msgSend;
     /**************************************************************************************/
     /* The task startRobot starts here                                                    */
     /**************************************************************************************/
+    while(1){
+        cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
+        // Synchronization barrier (waiting that all tasks are starting)
+        rt_sem_p(&sem_barrier, TM_INFINITE);
         //Boolean to get the battery
         rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
         killBatteryOk=0;
@@ -552,6 +645,7 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
             }
         }
     }
+}
 
 
 /**
@@ -560,24 +654,34 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
  * @brief Thread starting the communication with the robot.
  */
 void Tasks::StartRobotTaskWithWatchdog(void *arg) {
-    cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
+    
-    // Synchronization barrier (waiting that all tasks are starting)
-    rt_sem_p(&sem_barrier, TM_INFINITE);
     int killBatteryOk=0;
     int cpt=1;
     Message * msgSend;
+    int err;
+    
+    while(1){
+    
+        cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
+        // Synchronization barrier (waiting that all tasks are starting)
+        rt_sem_p(&sem_barrier, TM_INFINITE);
         /**************************************************************************************/
         /* The task startRobot starts here                                                    */
         /**************************************************************************************/
+
+
         //Boolean to get the battery
         rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
         killBattery=0;
         rt_mutex_release(&mutex_killBattery);
+
         rt_sem_p(&sem_startRobotWithWatchdog, TM_INFINITE);
         cout << "Start robot with watchdog (";
+
         rt_mutex_acquire(&mutex_robot, TM_INFINITE);
         msgSend = robot.Write(robot.StartWithWD());
         rt_mutex_release(&mutex_robot);
+
         cout << msgSend->GetID();
         cout << ")" << endl;  
         cout << "Movement answer: " << msgSend->ToString() << endl << flush;
@@ -587,23 +691,24 @@ void Tasks::StartRobotTaskWithWatchdog(void *arg) {
             rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
             robotStarted = 1;
             rt_mutex_release(&mutex_robotStarted);
+
             rt_task_set_periodic(NULL, TM_NOW, 500000000);
+
             while (killBatteryOk==0) {
                 cpt++;
                 if(cpt==2){
-                rt_mutex_acquire(&mutex_robot, TM_INFINITE);
-                robot.Write(robot.ReloadWD());
-                rt_mutex_release(&mutex_robot);
                     cpt=0;
                 }
                 rt_task_wait_period(NULL);
                 rt_sem_v(&sem_askBattery);
+
                 rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
                 killBatteryOk=killBattery;
                 rt_mutex_release(&mutex_killBattery);
             }
         }
     }
+}
 
 
 
@@ -644,6 +749,86 @@ void Tasks::MoveTask(void *arg) {
     }
 }
 
+
+void Tasks::DetectLostSupRob(void *arg){
+    //counter to detect loss of communication
+    int cpt=0;
+    bool kill_detectLostSupRobOk=0;
+    Message* msgSend;
+    
+    //Period = 1s
+    rt_task_set_periodic(NULL, TM_NOW, 1000000000);
+
+    while(1){
+    
+        //Wait the Communication with the Robot to be Set
+        rt_sem_p(&sem_detectLostSupRob, TM_INFINITE);
+        cout << "Start DetectLostSupRob" << endl << flush;
+        
+        //Initialize counter to detect loss
+        cpt = 0;
+        
+        //Initialize the variable for the loop condition
+        kill_detectLostSupRobOk = 0;        
+        rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
+            killDetectLostSupRob = 0;
+        rt_mutex_release(&mutex_killDetectLostSupRob);
+
+
+        while(!kill_detectLostSupRobOk){
+            rt_task_wait_period(NULL);
+
+            //Test Communication with Robot
+            rt_mutex_acquire(&mutex_robot, TM_INFINITE);
+                msgSend = robot.Write(robot.Ping());
+            rt_mutex_release(&mutex_robot);
+            cout << "J'écris un message" << endl << flush;
+            
+
+            if(msgSend->GetID() == MESSAGE_ANSWER_COM_ERROR || msgSend->GetID() == MESSAGE_ANSWER_ROBOT_TIMEOUT){
+
+                cout << "Didn't Get Any Answer from Robot" << endl << flush;
+                cpt++;
+
+                if(cpt==3){
+                    
+                    //acknowledge loss communication with robot
+                    //WriteInQueue(&q_messageToMon, new Message(MESSAGE_MONITOR_LOST));
+                    
+                    cout << "Restart Communication with Robot" << endl << flush;
+                    //Trigger Kill of DetectLostSupRob
+                    rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
+                        killDetectLostSupRob=1;
+                    rt_mutex_release(&mutex_killDetectLostSupRob);
+
+                    //Trigger Kill of the Battery acquisition and  therefore Robot with or without WD
+                    rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
+                        killBattery=1;
+                    rt_mutex_release(&mutex_killBattery);
+                    
+                    rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
+                        robotStarted=0;
+                    rt_mutex_release(&mutex_robotStarted);
+                    
+                    rt_sem_v(&sem_openComRobot);    
+
+
+                }
+            }
+
+            else{
+                cout << "I got an Answer from Robot" << endl << flush;
+            }
+
+            rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
+                kill_detectLostSupRobOk=killDetectLostSupRob;
+            rt_mutex_release(&mutex_killDetectLostSupRob);        
+        }
+        cout << "DetectLostSupRob dies";
+    }
+}
+
+        
 void Tasks::AskBattery(void *arg){
     Message* p_mess_answer_battery;
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;

software/raspberry/superviseur-robot/tasks.h

@@ -71,10 +71,11 @@ private:
     bool killVision=0;
     bool searchArena=0;
     bool killSendMon=0;
-    bool killDetectlostSupRob=0;
+    bool killDetectLostSupRob=0;
     bool acquireImage=0;
     bool getPosition=0;
     bool drawArena=0;
+    bool killOpenComRobot=0;
     
     
     /* Tasks                                                              */
@@ -86,6 +87,7 @@ private:
     RT_TASK th_startRobotWithoutWatchdog;
     RT_TASK th_startRobotWithWatchdog;
     RT_TASK th_move;
+    RT_TASK th_detectLostSupRob;
     RT_TASK th_vision;
     RT_TASK th_askBattery;
     
@@ -107,6 +109,8 @@ private:
     RT_MUTEX mutex_killDetectLostSupRob;
     RT_MUTEX mutex_killStartRob;
     RT_MUTEX mutex_acquireImage;
+    
+    RT_MUTEX mutex_killOpenComRobot;
     /**********************************************************************/
     /* Semaphores                                                         */
     /**********************************************************************/
@@ -115,9 +119,9 @@ private:
     RT_SEM sem_serverOk;
     RT_SEM sem_startRobotWithoutWatchdog;
     RT_SEM sem_startRobotWithWatchdog;
-    RT_SEM sem_allowStartReceive;
+    RT_SEM sem_detectLostSupRob;
+    RT_SEM sem_restart;
     RT_SEM sem_askBattery;
-    RT_SEM sem_allowStartCapture;
     
     /**********************************************************************/
     /* Message queues                                                     */
@@ -163,6 +167,10 @@ private:
      */
     void MoveTask(void *arg);
     
+    /**
+     * @brief Thread handling the loss of the communication between the robot and th supervisor.
+     */
+    void DetectLostSupRob(void *arg);
     
     
     void AskBattery(void *arg);
@@ -170,7 +178,6 @@ private:
     
     
     void Vision(void *arg);
-    
     /**********************************************************************/
     /* Queue services                                                     */
     /**********************************************************************/