Added Mutex and Thread management for arenaChoice.

software/raspberry/superviseur-robot/tasks.cpp

@@ -19,6 +19,7 @@
 #include <stdexcept>
 
 // Déclaration des priorités des taches
+// 99 -> haute, 0 -> basse
 #define PRIORITY_TSERVER 30
 #define PRIORITY_TOPENCOMROBOT 20
 #define PRIORITY_TMOVE 20
@@ -28,6 +29,7 @@
 #define PRIORITY_TCAMERA 21
 #define PRIORITY_TBATTERY 19
 #define PRIORITY_TSETCAMERA 18
+#define PRIOTITY_TARENA 17
 
 /*
  * Some remarks:
@@ -88,6 +90,10 @@ void Tasks::Init() {
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_mutex_create(&mutex_arenaStatus, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
     cout << "Mutexes created successfully" << endl << flush;
 
     /**************************************************************************************/
@@ -155,6 +161,10 @@ void Tasks::Init() {
         cerr << "Error task create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_task_create(&th_arenaChoice, "th_arenaChoice", 0, PRIOTITY_TARENA, 0)) {
+        cerr << "Error task create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
     cout << "Tasks created successfully" << endl << flush;
 
     /**************************************************************************************/
@@ -216,6 +226,10 @@ void Tasks::Run() {
         cerr << "Error task start: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_task_start(&th_arenaChoice, (void(*)(void*)) & Tasks::ArenaChoice, this)) {
+        cerr << "Error task start: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
 
     cout << "Tasks launched" << endl << flush;
 }
@@ -344,9 +358,10 @@ void Tasks::ReceiveFromMonTask(void *arg) {
             rt_mutex_release(&mutex_cameraStatus);
         }
         else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
-            std::cout << "\n\n\nArena asked !!!\n\n\n" << std::endl;
+            rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
-            cameraAsked();
+            if(cameraStatus == ArenaStatusEnum::NONE)
-            WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
+                cameraStatus = ArenaStatusEnum::SEARCHING;
+            rt_mutex_release(&mutex_arenaStatus);
         }
         else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) {
             std::cout << "\n\n\nArena Confirm asked !!!\n\n\n" << std::endl;
@@ -644,21 +659,43 @@ void Tasks::CloseCamera(void * arg)
     }
 }
 
-void Tasks::cameraAsked()
+void Tasks::ArenaChoice(void * arg)
 {
-    Img * img = new Img(cam->Grab());
+    cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
-    Arena a = img->SearchArena();
+    // Synchronization barrier (waiting that all tasks are starting)
+    rt_sem_p(&sem_barrier, TM_INFINITE);
+    Img * img = nullptr;
+    Arena a;
+    ArenaStatusEnum as = ArenaStatusEnum::NONE;
     
-    std::cout << "\n\n\n" << img->ToString() << "\n\n\n" << std::endl;
+    while(1)
-    if(a.IsEmpty())
     {
-        std::cout << "\n\n\nArena null :(\n\n\n" << std::endl;
+        // Check the status of the arena
-        WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
+        rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
+        as = arenaStatus;
+        rt_mutex_release(&mutex_arenaStatus);
+        if(as == ArenaStatusEnum::SEARCHING)
+        {   
+            // Gathering last image + closing camera when prompted
+            rt_mutex_acquire(&mutex_camera, TM_INFINITE);
+            if(cam->IsOpen())
+            {
+                img = new Img(cam->Grab());
+                cam->Close();
             }
+            rt_mutex_release(&mutex_camera);
+            
+            // Putting the arena overlay if found
+            a = img->SearchArena();
+            if(a.IsEmpty())
+                WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
             else {
-        std::cout << "\n\n\nArena Image sending :)\n\n\n" << std::endl;
                 img->DrawArena(a);
                 MessageImg *msgImg = new MessageImg(MESSAGE_CAM_IMAGE, img);
+                rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
                 monitor.Write(msgImg);
+                rt_mutex_release(&mutex_monitor);
+            }
+        }
     }
 }

software/raspberry/superviseur-robot/tasks.h

@@ -43,6 +43,12 @@ enum class CameraStatusEnum {
     CLOSING
 };
 
+enum class ArenaStatusEnum {
+    NONE,
+    SEARCHING,
+    CHOOSEN
+};
+
 class Tasks {
 public:
     /**
@@ -75,6 +81,7 @@ private:
     int move = MESSAGE_ROBOT_STOP;
     bool robotBatteryGet = false;
     CameraStatusEnum cameraStatus = CameraStatusEnum::CLOSED;
+    ArenaStatusEnum arenaStatus = ArenaStatusEnum::NONE;
     Camera* cam = nullptr;
 
     /**********************************************************************/
@@ -90,6 +97,7 @@ private:
     RT_TASK th_cameraOpen;
     RT_TASK th_cameraImage;
     RT_TASK th_cameraClose;
+    RT_TASK th_arenaChoice;
 
     /**********************************************************************/
     /* Mutex                                                              */
@@ -101,6 +109,7 @@ private:
     RT_MUTEX mutex_batteryGet;
     RT_MUTEX mutex_cameraStatus;
     RT_MUTEX mutex_camera;
+    RT_MUTEX mutex_arenaStatus;
 
     /**********************************************************************/
     /* Semaphores                                                         */
@@ -156,7 +165,7 @@ private:
     void OpenCamera(void * arg);
     void ImageCamera(void * arg);
     void CloseCamera(void * arg);
-    
+    void ArenaChoice(void * arg);
     // Utility functions
 
 
@@ -178,7 +187,7 @@ private:
      */
     Message *ReadInQueue(RT_QUEUE *queue);
     
-    void cameraAsked();
+