Create Task ReceiveFromMon

software/raspberry/superviseur-robot/main.cpp

@@ -36,6 +36,8 @@ int main(int argc, char **argv) {
     cout<<"#      DE STIJL PROJECT         #"<<endl;
     cout<<"#################################"<<endl;
 
+    //realease Sem just the first time
+    
     tasks.Init();
     tasks.Run();
     tasks.Join();

software/raspberry/superviseur-robot/tasks.cpp

@@ -75,6 +75,59 @@ void Tasks::Init() {
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_mutex_create(&mutex_killReceiveFromMon, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    
+    if (err = rt_mutex_create(&mutex_killVision, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    
+    if (err = rt_mutex_create(&mutex_searchArena, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+
+    if (err = rt_mutex_create(&mutex_drawArena, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    
+    if (err = rt_mutex_create(&mutex_getPosition, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+
+    if (err = rt_mutex_create(&mutex_killSendMon, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+
+    if (err = rt_mutex_create(&mutex_killStartRobWD, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }    
+
+    if (err = rt_mutex_create(&mutex_killDetectLostSupRob, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    } 
+
+    if (err = rt_mutex_create(&mutex_killStartRob, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
+    
+    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_robot_on, NULL)) {
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
@@ -106,6 +159,16 @@ void Tasks::Init() {
         cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+
+    if (err = rt_sem_create(&sem_allow_StartReceive, 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_allow_StartReceive);
+    
     cout << "Semaphores created successfully" << endl << flush;
 
     /**************************************************************************************/
@@ -265,18 +328,27 @@ void Tasks::SendToMonTask(void* arg) {
  */
 void Tasks::ReceiveFromMonTask(void *arg) {
     Message *msgRcv;
+    bool Ok=1;
     
     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
+    rt_sem_p(&sem_allow_StartReceive, TM_INFINITE);
+    
+    //Reinitialize control boolean 
+    rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
+        killReceiveFromMon=0;
+    rt_mutex_release(&mutex_killReceiveFromMon);
+    
     /**************************************************************************************/
     /* The task receiveFromMon starts here                                                */
     /**************************************************************************************/
     rt_sem_p(&sem_serverOk, TM_INFINITE);
     cout << "Received message from monitor activated" << endl << flush;
 
-    while (1) {
+    while (Ok) {
         msgRcv = monitor.Read();
         cout << "Rcv <= " << msgRcv->ToString() << endl << flush;
 
@@ -284,14 +356,87 @@ void Tasks::ReceiveFromMonTask(void *arg) {
             delete(msgRcv);
             cout << "Connection to monitor lost" << endl;
             monitor.Close();
+            
+            rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
+                killReceiveFromMon=1;
+            rt_mutex_release(&mutex_killReceiveFromMon);
+            
+            rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
+                killVision=1;
+            rt_mutex_release(&mutex_killVision);
+            
+            rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE);
+                killSendMon=1;
+            rt_mutex_release(&mutex_killSendMon);     
+            
+            rt_mutex_acquire(&mutex_killStartRobWD, TM_INFINITE);
+                killStartRobWD=1;
+            rt_mutex_release(&mutex_killStartRobWD);            
+             
+            rt_mutex_acquire(&mutex_killStartRob, TM_INFINITE);
+                killStartRob=1;
+            rt_mutex_release(&mutex_killStartRob);
+
+            rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE);
+                killDetectlostSupRob=1;
+            rt_mutex_release(&mutex_killDetectLostSupRob);
+            
+            rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
+                acquireImage=0;
+            rt_mutex_release(&mutex_acquireImage);            
+            
             //exit(-1);
         
+        
+        } else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
+            //start task Vision
+            
         } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
             rt_sem_v(&sem_openComRobot);
+            
         } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
             rt_sem_v(&sem_startRobotWithoutWatchdog);
+            
         } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
-            rt_sem_v(&sem_startRobotWithWatchdog);
+            //start task robot with watchdog 
+
+        } else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
+            rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
+                searchArena=1;
+            rt_mutex_release(&mutex_searchArena);
+            
+        } else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) {
+            rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
+                getPosition=1;
+            rt_mutex_release(&mutex_getPosition);            
+
+        } else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)) {
+            rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
+                getPosition=0;
+            rt_mutex_release(&mutex_getPosition); 
+
+        } else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) {
+            rt_mutex_acquire(&mutex_drawArena, TM_INFINITE);
+                drawArena=1;
+            rt_mutex_release(&mutex_drawArena);
+            
+            rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
+                searchArena=0;
+            rt_mutex_release(&mutex_searchArena);
+           
+            rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
+                acquireImage=1;
+            rt_mutex_release(&mutex_acquireImage);            
+ 
+        } else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) {           
+            rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
+                searchArena=0;
+            rt_mutex_release(&mutex_searchArena);
+            
+            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) ||
@@ -302,8 +447,15 @@ void Tasks::ReceiveFromMonTask(void *arg) {
             move = msgRcv->GetID();
             rt_mutex_release(&mutex_move);
         }
-        delete(msgRcv); // mus be deleted manually, no consumer
+        delete(msgRcv); // must be deleted manually, no consumer
+        
+        rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE);
+        Ok=!killReceiveFromMon;
+        rt_mutex_release(&mutex_killReceiveFromMon);
+        
     }
+    
+    rt_sem_v(&sem_allow_StartReceive);
 }
 
 /**
@@ -359,9 +511,9 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
     cout << "Start robot without watchdog (";
     
     //Boolean to get the battery
-    rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE);
+    rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
-    killBatteryBool=0;
+    killBattery=0;
-    rt_mutex_release(&mutex_kill_battery);
+    rt_mutex_release(&mutex_killBattery);
     rt_mutex_acquire(&mutex_robot, TM_INFINITE);
     msgSend = robot.Write(robot.StartWithoutWD());
     rt_mutex_release(&mutex_robot);
@@ -384,9 +536,9 @@ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) {
             monitor.Write(p_mess_answer_battery);
             rt_mutex_release(&mutex_monitor);
             //cout << endl << flush;
-            rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE);
+            rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
-            killBattery=killBatteryBool;
+            killBattery=killBattery;
-            rt_mutex_release(&mutex_kill_battery);
+            rt_mutex_release(&mutex_killBattery);
         }
     }
 }
@@ -417,9 +569,9 @@ void Tasks::StartRobotTaskWithWatchdog(void *arg) {
     rt_mutex_release(&mutex_robot);
     
     //boolean to ask battery
-    rt_mutex_acquire(&mutex_kill_battery, TM_INFINITE);
+    rt_mutex_acquire(&mutex_killBattery, TM_INFINITE);
-    killBatteryBool=0;
+    killBattery=0;
-    rt_mutex_release(&mutex_kill_battery);
+    rt_mutex_release(&mutex_killBattery);
     
     cout << msgSend->GetID();
     cout << ")" << endl;

software/raspberry/superviseur-robot/tasks.h

@@ -65,9 +65,20 @@ private:
     ComMonitor monitor;
     ComRobot robot;
     int robotStarted = 0;
-    int killBatteryBool = 0;
+    int killBattery = 0;
-    int killRobotWithoutWd = 0;
     int move = MESSAGE_ROBOT_STOP;
+    bool killReceiveFromMon=0;
+    bool killVision=0;
+    bool searchArena=0;
+    bool killSendMon=0;
+    bool killStartRobWD=0; 
+    bool killStartRob=0;
+    bool killDetectlostSupRob=0;
+    bool acquireImage=0;
+    bool getPosition=0;
+    bool drawArena=0;
+    
+    
     
     /**********************************************************************/
     /* Tasks                                                              */
@@ -87,7 +98,17 @@ private:
     RT_MUTEX mutex_robot;
     RT_MUTEX mutex_robotStarted;
     RT_MUTEX mutex_move;
-    RT_MUTEX mutex_kill_battery;
+    RT_MUTEX mutex_killBattery;
+    RT_MUTEX mutex_killReceiveFromMon;
+    RT_MUTEX mutex_killVision;
+    RT_MUTEX mutex_searchArena;
+    RT_MUTEX mutex_drawArena;
+    RT_MUTEX mutex_getPosition;
+    RT_MUTEX mutex_killSendMon;
+    RT_MUTEX mutex_killStartRobWD;
+    RT_MUTEX mutex_killDetectLostSupRob;
+    RT_MUTEX mutex_killStartRob;
+    RT_MUTEX mutex_acquireImage;
     
     /**********************************************************************/
     /* Semaphores                                                         */
@@ -97,6 +118,7 @@ private:
     RT_SEM sem_serverOk;
     RT_SEM sem_startRobotWithoutWatchdog;
     RT_SEM sem_startRobotWithWatchdog;
+    RT_SEM sem_allow_StartReceive;
     
     /**********************************************************************/
     /* Message queues                                                     */