Modified detectLostSupRob

software/raspberry/superviseur-robot/tasks.cpp

@@ -114,7 +114,12 @@ void Tasks::Init() {
     if (err = rt_mutex_create(&mutex_acquireImage, NULL)) {
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
-    }     
+    }
+
+    if (err = rt_mutex_create(&mutex_killOpenComRobot, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }      
 
     
     
@@ -161,14 +166,31 @@ void Tasks::Init() {
         exit(EXIT_FAILURE);
     } 
     
-     if (err = rt_sem_create(&sem_DetectLostSupRob, NULL, 0, S_FIFO)) {
+     if (err = rt_sem_create(&sem_detectLostSupRob, NULL, 0, S_FIFO)) {
+        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);
+    }
+
+     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;
 
@@ -226,44 +248,49 @@ void Tasks::Init() {
  * @brief Démarrage des tâches
  */
 void Tasks::Run() {
-    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;
-        exit(EXIT_FAILURE);
-    }
-    if (err = rt_task_start(&th_sendToMon, (void(*)(void*)) & Tasks::SendToMonTask, this)) {
-        cerr << "Error task start: " << 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;
-        exit(EXIT_FAILURE);
-    }
-    if (err = rt_task_start(&th_openComRobot, (void(*)(void*)) & Tasks::OpenComRobot, this)) {
-        cerr << "Error task start: " << 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;
-        exit(EXIT_FAILURE);
-    }
-    if (err = rt_task_start(&th_startRobotWithWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithWatchdog, this)) {
-        cerr << "Error task start: " << 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;
-        exit(EXIT_FAILURE);
-    }
-
-    if (err = rt_task_start(&th_detectLostSupRob, (void(*)(void*)) & Tasks::DetectLostSupRob, this)) {
-        cerr << "Error task start: " << strerror(-err) << endl << flush;
-        exit(EXIT_FAILURE);
-    }    
     
-    cout << "Tasks launched" << endl << flush;
+    while(1){
+        rt_sem_p(&sem_restart, TM_INFINITE);
+
+        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;
+            exit(EXIT_FAILURE);
+        }
+        if (err = rt_task_start(&th_sendToMon, (void(*)(void*)) & Tasks::SendToMonTask, this)) {
+            cerr << "Error task start: " << 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;
+            exit(EXIT_FAILURE);
+        }
+        if (err = rt_task_start(&th_openComRobot, (void(*)(void*)) & Tasks::OpenComRobot, this)) {
+            cerr << "Error task start: " << 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;
+            exit(EXIT_FAILURE);
+        }
+        if (err = rt_task_start(&th_startRobotWithWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithWatchdog, this)) {
+            cerr << "Error task start: " << 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;
+            exit(EXIT_FAILURE);
+        }
+
+        if (err = rt_task_start(&th_detectLostSupRob, (void(*)(void*)) & Tasks::DetectLostSupRob, this)) {
+            cerr << "Error task start: " << strerror(-err) << endl << flush;
+            exit(EXIT_FAILURE);
+        }    
+
+        cout << "Tasks launched" << endl << flush;
+    }
 }
 
 /**
@@ -314,24 +341,40 @@ void Tasks::ServerTask(void *arg) {
  */
 void Tasks::SendToMonTask(void* arg) {
     Message *msg;
+    bool kill_sendToMonOk=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_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);
 
-    while (1) {
+    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 << "Task SendToMon dies" << endl << flush;
+    rt_sem_v(&sem_allowSendToMon);
 }
 
 /**
@@ -390,7 +433,10 @@ void Tasks::ReceiveFromMonTask(void *arg) {
             
             rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
                 acquireImage=0;
-            rt_mutex_release(&mutex_acquireImage);            
+            rt_mutex_release(&mutex_acquireImage);
+            
+            //Restart all the process
+            rt_sem_v(&sem_restart);
             
             //exit(-1);
         
@@ -418,7 +464,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);            
@@ -472,6 +518,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
         
     }
     
+    cout << "Task ReceiveFromMon dies" << __PRETTY_FUNCTION__ << endl << flush;
     rt_sem_v(&sem_allowStartReceive);
 }
 
@@ -481,15 +528,24 @@ void Tasks::ReceiveFromMonTask(void *arg) {
 void Tasks::OpenComRobot(void *arg) {
     int status;
     int err;
+    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 (1) {
+    while (!killOpenComRobot) {
         rt_sem_p(&sem_openComRobot, TM_INFINITE);
         cout << "Open serial com (";
         rt_mutex_acquire(&mutex_robot, TM_INFINITE);
@@ -507,8 +563,15 @@ void Tasks::OpenComRobot(void *arg) {
         WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
         
         //Trigger Detection of Communication Loss with Robot
-        rt_sem_v(&sem_DetectLostSupRob);
+        rt_sem_v(&sem_detectLostSupRob);
+        
+        rt_mutex_acquire(&mutex_killOpenComRobot, TM_INFINITE);
+            killOpenComRobotOk = killOpenComRobot;
+        rt_mutex_release(&mutex_killOpenComRobot);
+        
     }
+    
+    rt_sem_v(&sem_allowOpenComRobot);
 }
 
 /**
@@ -677,14 +740,18 @@ void Tasks::DetectLostSupRob(void *arg){
     rt_sem_p(&sem_allowStartDetectLostSupRob, TM_INFINITE);
     
     //Wait the Communication with the Robot to be Set
-    rt_sem_p(&sem_DetectLostSupRob, TM_INFINITE);
+    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, 500000000);
+    
     while(!kill_detectLostSupRobOk){
+        rt_task_wait_period(NULL);
         
         //Test Communication with Robot
         rt_mutex_acquire(&mutex_robot, TM_INFINITE);

software/raspberry/superviseur-robot/tasks.h

@@ -75,6 +75,7 @@ private:
     bool acquireImage=0;
     bool getPosition=0;
     bool drawArena=0;
+    bool killOpenComRobot=0;
     
     
     
@@ -108,6 +109,7 @@ private:
     RT_MUTEX mutex_killDetectLostSupRob;
     RT_MUTEX mutex_killStartRob;
     RT_MUTEX mutex_acquireImage;
+    RT_MUTEX mutex_killOpenComRobot;
     
     /**********************************************************************/
     /* Semaphores                                                         */
@@ -119,7 +121,10 @@ private:
     RT_SEM sem_startRobotWithWatchdog;
     RT_SEM sem_allowStartReceive;
     RT_SEM sem_allowStartDetectLostSupRob;
-    RT_SEM sem_DetectLostSupRob;
+    RT_SEM sem_detectLostSupRob;
+    RT_SEM sem_restart;
+    RT_SEM sem_allowSendToMon;
+    RT_SEM sem_allowOpenComRobot;
     
     /**********************************************************************/
     /* Message queues                                                     */