Restart detectLostSupMon ok but ping doesn't work after restart

software/raspberry/superviseur-robot/tasks.cpp

@@ -368,15 +368,16 @@ void Tasks::ReceiveFromMonTask(void *arg) {
     
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
         
+    // Synchronization barrier (waiting that all tasks are starting)
+    rt_sem_p(&sem_barrier, TM_INFINITE);
+    
     while(1){
-        // Synchronization barrier (waiting that all tasks are starting)
-        rt_sem_p(&sem_barrier, TM_INFINITE);
-
         //rt_sem_p(&sem_restart,TM_INFINITE);
 
-        //Reinitialize control boolean 
+        //Reinitialize control boolean
+        killReceiveFromMonOk = 0;
         rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
-            killReceiveFromMon=0;
+            killReceiveFromMon = 0;
         rt_mutex_release(&mutex_killReceiveFromMon);
 
         /**************************************************************************************/
@@ -390,7 +391,6 @@ void Tasks::ReceiveFromMonTask(void *arg) {
             cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
 
             if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
-                delete(msgRcv);
                 cout << "Connection to monitor lost" << endl;
 
                 rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
@@ -419,16 +419,19 @@ void Tasks::ReceiveFromMonTask(void *arg) {
                     acquireImage=0;
                 rt_mutex_release(&mutex_acquireImage);
 
+                //Wait every task to die
                 sleep(1);
-
+                
+                //All closes
                 monitor.Close();
                 robot.Close();
 
                 //Tasks::Join();
                 
-                cout << "End sleep" << endl << flush;
+                //Release restarted tasks
+//                cout << "End sleep" << endl << flush;
                 rt_sem_broadcast(&sem_barrier);             
-                cout << "Mes couilles" << endl << flush;
+//                cout << "Mes couilles" << endl << flush;
 
                 //exit(-1);
 
@@ -518,13 +521,9 @@ void Tasks::ReceiveFromMonTask(void *arg) {
 
             //Update loop condition
 
-            cout << "J'arrive la" << endl << flush;
-
             rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
                 killReceiveFromMonOk = killReceiveFromMon;
             rt_mutex_release(&mutex_killReceiveFromMon);
-
-            cout << "Kill Receive From Mon Ok = " << killReceiveFromMonOk << endl << flush;
         }
 
         cout << "ReceiveFromMon dies" << endl << flush;
@@ -642,8 +641,6 @@ void Tasks::StartRobotTaskWithWatchdog(void *arg) {
     int err;
     
     while(1){
-    
-        rt_sem_p(&sem_restart, TM_INFINITE);
         
         rt_sem_p(&sem_startRobotWithWatchdog, TM_INFINITE);
         cout << "Start robot with watchdog (";
@@ -734,53 +731,61 @@ void Tasks::DetectLostSupRob(void *arg){
     bool kill_detectLostSupRobOk=0;
     Message* msgSend;
     
-    //Wait the Communication with the Robot to be Set
-    rt_sem_p(&sem_detectLostSupRob, TM_INFINITE);
-    
-    //Initialize the variable for the loop condition
-    rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
-        killDetectLostSupRob=0;
-    rt_mutex_release(&mutex_killDetectLostSupRob);
-    
     //Period = 1s
     rt_task_set_periodic(NULL, TM_NOW, 1000000000);
+
+    while(1){
     
-    while(!kill_detectLostSupRobOk){
+        //Wait the Communication with the Robot to be Set
-        rt_task_wait_period(NULL);
+        rt_sem_p(&sem_detectLostSupRob, TM_INFINITE);
-        
+        cout << "Start DetectLostSupRob" << endl << flush;
-        //Test Communication with Robot
+
-        rt_mutex_acquire(&mutex_robot, TM_INFINITE);
+        kill_detectLostSupRobOk = 0;
-            msgSend = robot.Write(robot.Ping());
+        //Initialize the variable for the loop condition
-        rt_mutex_release(&mutex_robot);
-        
-        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){
-                //Trigger Kill of DetectLostSupRob
-                rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
-                    killDetectLostSupRob=1;
-                rt_mutex_release(&mutex_killDetectLostSupRob);
-                
-                //Trigger Kill of the Battery acquisition
-                rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
-                    killBattery=1;
-                rt_mutex_release(&mutex_killBattery);  
-               
-            }
-        }
-        
-        else{
-            cout << "I got an Answer from Robot" << endl << flush;
-        }
-        
         rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
-            kill_detectLostSupRobOk=killDetectLostSupRob;
+            killDetectLostSupRob = 0;
-        rt_mutex_release(&mutex_killDetectLostSupRob);        
+        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){
+                    //Trigger Kill of DetectLostSupRob
+                    rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
+                        killDetectLostSupRob=1;
+                    rt_mutex_release(&mutex_killDetectLostSupRob);
+
+                    //Trigger Kill of the Battery acquisition
+                    rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
+                        killBattery=1;
+                    rt_mutex_release(&mutex_killBattery);  
+
+                }
+            }
+
+            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";
     }
-    cout << "DetectLostSupRob dies";
 }
 
 /**