Restart receiveFromMon doesn't work

software/raspberry/superviseur-robot/tasks.cpp

@@ -19,11 +19,11 @@
 #include <stdexcept>
 
 // Déclaration des priorités des taches
-#define PRIORITY_TSERVER 30
-#define PRIORITY_TOPENCOMROBOT 20
+#define PRIORITY_TSERVER 10
+#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
@@ -156,16 +156,6 @@ void Tasks::Init() {
         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_allowStartDetectLostSupRob, NULL, 0, S_FIFO)) {
-        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
-        exit(EXIT_FAILURE);
-    } 
-    
      if (err = rt_sem_create(&sem_detectLostSupRob, NULL, 0, S_FIFO)) {
         cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
@@ -176,58 +166,42 @@ void Tasks::Init() {
         exit(EXIT_FAILURE);
     } 
     
-     if (err = rt_sem_create(&sem_allowSendToMon, NULL, 0, S_FIFO)) {
-        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
-        exit(EXIT_FAILURE);
-    }
-    
-     if (err = rt_sem_create(&sem_allowOpenComRobot, 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
-    rt_sem_v(&sem_allowStartReceive);
-    rt_sem_v(&sem_allowStartDetectLostSupRob);
-    rt_sem_v(&sem_restart);
-    rt_sem_v(&sem_allowOpenComRobot);
-    
     cout << "Semaphores created successfully" << endl << flush;
 
     /**************************************************************************************/
     /* 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: " << strerror(-err) << endl << flush;
+        cerr << "Error task create detectLostSupRob: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
     
@@ -249,48 +223,47 @@ void Tasks::Init() {
  */
 void Tasks::Run() {
     
-    while(1){
-        rt_sem_p(&sem_restart, TM_INFINITE);
+        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: " << strerror(-err) << endl << flush;
+            cerr << "Error task start detectLostSupRob: " << strerror(-err) << endl << flush;
             exit(EXIT_FAILURE);
         }    
 
         cout << "Tasks launched" << endl << flush;
-    }
+
 }
 
 /**
@@ -306,6 +279,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();
 }
 
@@ -316,6 +293,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);
 
@@ -334,6 +313,7 @@ void Tasks::ServerTask(void *arg) {
         monitor.AcceptClient(); // Wait the monitor client
         cout << "Rock'n'Roll baby, client accepted!" << endl << flush;
         rt_sem_broadcast(&sem_serverOk);
+    }
 }
 
 /**
@@ -344,21 +324,23 @@ void Tasks::SendToMonTask(void* arg) {
     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);
     
-    rt_sem_p(&sem_allowSendToMon, TM_INFINITE);
-    
-    //Initialize the loop condition
-    rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE);
-        killSendMon=0; // The message is deleted with the Write
-    rt_mutex_release(&mutex_killSendMon);
 
         /**************************************************************************************/
         /* The task sendToMon starts here                                                     */
         /**************************************************************************************/
         rt_sem_p(&sem_serverOk, TM_INFINITE);
         
+        //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);
@@ -373,8 +355,8 @@ void Tasks::SendToMonTask(void* arg) {
 
         }
         
-    cout << "Task SendToMon dies" << endl << flush;
-    rt_sem_v(&sem_allowSendToMon);
+        cout << "SendToMon Task dies" << endl << flush;
+    }
 }
 
 /**
@@ -385,11 +367,12 @@ void Tasks::ReceiveFromMonTask(void *arg) {
     bool killReceiveFromMonOk=0;
     
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
+        
+    while(1){
         // Synchronization barrier (waiting that all tasks are starting)
         rt_sem_p(&sem_barrier, TM_INFINITE);
 
-    //Wait twin task to die if not first round
-    rt_sem_p(&sem_allowStartReceive, TM_INFINITE);
+        //rt_sem_p(&sem_restart,TM_INFINITE);
 
         //Reinitialize control boolean 
         rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
@@ -409,7 +392,6 @@ void Tasks::ReceiveFromMonTask(void *arg) {
             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;
@@ -422,6 +404,8 @@ 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;
@@ -435,20 +419,41 @@ void Tasks::ReceiveFromMonTask(void *arg) {
                     acquireImage=0;
                 rt_mutex_release(&mutex_acquireImage);
 
-            //Restart all the process
-            rt_sem_v(&sem_restart);
+                sleep(1);
+
+                monitor.Close();
+                robot.Close();
+
+                //Tasks::Join();
+                
+                cout << "End sleep" << endl << flush;
+                rt_sem_broadcast(&sem_barrier);             
+                cout << "Mes couilles" << endl << flush;
 
                 //exit(-1);
 
 
             } 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_START_WITHOUT_WD)) {
                 cout << "Command Start Robot without Watchdog Received" << endl << flush;
                 rt_sem_v(&sem_startRobotWithoutWatchdog);
@@ -512,40 +517,36 @@ void Tasks::ReceiveFromMonTask(void *arg) {
             delete(msgRcv); // must be deleted manually, no consumer
 
             //Update loop condition
+
+            cout << "J'arrive la" << endl << flush;
+
             rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
-        killReceiveFromMonOk=killReceiveFromMon;
+                killReceiveFromMonOk = killReceiveFromMon;
             rt_mutex_release(&mutex_killReceiveFromMon);
 
+            cout << "Kill Receive From Mon Ok = " << killReceiveFromMonOk << endl << flush;
         }
 
-    cout << "Task ReceiveFromMon dies" << __PRETTY_FUNCTION__ << endl << flush;
-    rt_sem_v(&sem_allowStartReceive);
+        cout << "ReceiveFromMon dies" << endl << flush;
+    }
 }
 
 /**
  * @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;
+    //bool killOpenComRobotOk = 0;
 
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
     // Synchronization barrier (waiting that all tasks are starting)
     rt_sem_p(&sem_barrier, TM_INFINITE);
 
-    rt_sem_p(&sem_allowOpenComRobot, TM_INFINITE);
-    
-    rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE);
-        killOpenComRobot = 0;
-    rt_mutex_release(&mutex_killOpenComRobot);
-
-    rt_sem_p(&sem_openComRobot, TM_INFINITE);
-    
         /**************************************************************************************/
         /* The task openComRobot starts here                                                  */
         /**************************************************************************************/
-    while (!killOpenComRobot) {
+        while (1) {
             rt_sem_p(&sem_openComRobot, TM_INFINITE);    
             cout << "Open serial com (";
             rt_mutex_acquire(&mutex_robot, TM_INFINITE);
@@ -565,13 +566,7 @@ void Tasks::OpenComRobot(void *arg) {
             //Trigger Detection of Communication Loss with Robot
             rt_sem_v(&sem_detectLostSupRob);
 
-        rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE);
-            killOpenComRobotOk = killOpenComRobot;
-        rt_mutex_release(&mutex_killOpenComRobot);
-        
         }
-    
-    rt_sem_v(&sem_allowOpenComRobot);
 }
 
 /**
@@ -624,9 +619,6 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
             rt_mutex_release(&mutex_killBattery);
         }
     }
-    
-    //Trigger New Opening of the Communication with the Robot
-    //rt_sem_v(&sem_openComRobot);
 }
 
 
@@ -648,6 +640,11 @@ void Tasks::StartRobotTaskWithWatchdog(void *arg) {
     Message * msgSend;
     int cpt=1;
     int err;
+    
+    while(1){
+    
+        rt_sem_p(&sem_restart, TM_INFINITE);
+        
         rt_sem_p(&sem_startRobotWithWatchdog, TM_INFINITE);
         cout << "Start robot with watchdog (";
         rt_mutex_acquire(&mutex_robot, TM_INFINITE);
@@ -688,6 +685,7 @@ void Tasks::StartRobotTaskWithWatchdog(void *arg) {
                 cout << endl << flush;
             }
         }
+    }
 }
 
 
@@ -736,9 +734,6 @@ void Tasks::DetectLostSupRob(void *arg){
     bool kill_detectLostSupRobOk=0;
     Message* msgSend;
     
-    //garanties twin task is dead if applicable
-    rt_sem_p(&sem_allowStartDetectLostSupRob, TM_INFINITE);
-    
     //Wait the Communication with the Robot to be Set
     rt_sem_p(&sem_detectLostSupRob, TM_INFINITE);
     
@@ -748,7 +743,7 @@ void Tasks::DetectLostSupRob(void *arg){
     rt_mutex_release(&mutex_killDetectLostSupRob);
     
     //Period = 1s
-    rt_task_set_periodic(NULL, TM_NOW, 500000000);
+    rt_task_set_periodic(NULL, TM_NOW, 1000000000);
     
     while(!kill_detectLostSupRobOk){
         rt_task_wait_period(NULL);
@@ -766,12 +761,12 @@ void Tasks::DetectLostSupRob(void *arg){
             if(cpt==3){
                 //Trigger Kill of DetectLostSupRob
                 rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
-                    killDetectLostSupRob=0;
+                    killDetectLostSupRob=1;
                 rt_mutex_release(&mutex_killDetectLostSupRob);
                 
                 //Trigger Kill of the Battery acquisition
                 rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
-                    killBattery=0;
+                    killBattery=1;
                 rt_mutex_release(&mutex_killBattery);  
                
             }
@@ -785,8 +780,7 @@ void Tasks::DetectLostSupRob(void *arg){
             kill_detectLostSupRobOk=killDetectLostSupRob;
         rt_mutex_release(&mutex_killDetectLostSupRob);        
     }
-         
-    rt_sem_v(&sem_allowStartDetectLostSupRob);
+    cout << "DetectLostSupRob dies";
 }
 
 /**

software/raspberry/superviseur-robot/tasks.h

@@ -119,12 +119,8 @@ private:
     RT_SEM sem_serverOk;
     RT_SEM sem_startRobotWithoutWatchdog;
     RT_SEM sem_startRobotWithWatchdog;
-    RT_SEM sem_allowStartReceive;
-    RT_SEM sem_allowStartDetectLostSupRob;
     RT_SEM sem_detectLostSupRob;
     RT_SEM sem_restart;
-    RT_SEM sem_allowSendToMon;
-    RT_SEM sem_allowOpenComRobot;
     
     /**********************************************************************/
     /* Message queues                                                     */