draft Gwen camera jusqu'aux robot

.idea/.gitignore

@@ -0,0 +1,8 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Editor-based HTTP Client requests
+/httpRequests/
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml

.idea/misc.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ExternalStorageConfigurationManager" enabled="true" />
+  <component name="MakefileSettings">
+    <option name="linkedExternalProjectsSettings">
+      <MakefileProjectSettings>
+        <option name="externalProjectPath" value="$PROJECT_DIR$/software/raspberry/superviseur-robot" />
+        <option name="modules">
+          <set>
+            <option value="$PROJECT_DIR$/software/raspberry/superviseur-robot" />
+          </set>
+        </option>
+        <option name="version" value="2" />
+      </MakefileProjectSettings>
+    </option>
+  </component>
+  <component name="MakefileWorkspace" PROJECT_DIR="$PROJECT_DIR$/software/raspberry/superviseur-robot">
+    <contentRoot DIR="$PROJECT_DIR$" />
+  </component>
+</project>

.idea/vcs.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="" vcs="Git" />
+  </component>
+</project>

software/raspberry/superviseur-robot/tasks.cpp

@@ -285,6 +285,11 @@ void Tasks::ReceiveFromMonTask(void *arg) {
             rt_mutex_release(&mutex_move);
         } else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)){
             rt_sem_v(&sem_startCamera);
+        } else if(msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) || msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)){
+            rt_mutex_acquire(&mutex_answer_arena);
+            msg_arena = msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM? 1:0; // A vérifier que le ternaire est utile 
+            rt_mutex_release(&mutex_answer_arena);
+            rt_sem_v(&sem_answerSync); 
         }
         delete(msgRcv); // must be deleted manually, no consumer
     }
@@ -365,7 +370,7 @@ void Tasks::StartCameraTask(void *arg) {
     rt_sem_p(&sem_barrier, TM_INFINITE);
     
     /**************************************************************************************/
-    /* The task startRobot starts here                                                    */
+    /* The task Camera Task starts here                                                    */
     /**************************************************************************************/
     while (1) {
 
@@ -467,26 +472,79 @@ void Tasks::BatteryTask(void *arg) {
 */
 void CameraTask(void *arg){
     int cam;  
+    std::list<Position> robotPosition;
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
     // Synchronization barrier (waiting that all tasks are starting)
     rt_sem_p(&sem_barrier, TM_INFINITE);
     rt_task_set_periodic(NULL, TM_NOW, 100*1000*1000);
     while(1){
         rt_task_wait_period(NULL);
-        rt_mutex_acquire(&mutex_camera, TM_INFINITE);
+        rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
         cam = cameraStarted;
+        rt_mutex_release(&mutex_cameraStarted); 
         
-        if(cam){
+        if(!cam){
+            continue;
+        }
+
+        rt_mutex_acquire(&mutex_camera, TM_INFINITE);
         // Lancer le traitement périodique 
+        Img img = camera.Grab(); 
+        Arena arena= img.SearchArena(); // Mise a jour de l'arene tout le temps pour la position du robot mais draw si search arena = true 
         rt_mutex_release(&mutex_camera);
-        }else{
+        rt_mutex_acquire(&mutex_search_arena, TM_INFINITE); 
-            rt_mutex_release(&mutex_camera);
+        if(searchArena){
+            // chopper l'arene 
+            cout << "Search arena pls ! " << endl << flush;
+            if(arena.IsEmpty()){
+                cout << "Arena empty" << endl << flush;
+                // envoi d'un no ack au moniteur 
 
-            rt_mutex_acquire(&mutex, TM_INFINITE);
+            }else{
-            rt_mutex_release(&mutex_camera);
+                cout << "Draw Arena" << endl << flush;
+                img.DrawArena(arena);
+
+                // envoi de l'arene pour vérif
+                cout << "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAARENE" << endl << flush;
+            }
+        }
+        rt_mutex_release(&mutex_search_arena); 
+        rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); 
+        
+        if(robotPos){
+            // chopper le robot
+            cout << "Pos du robot pls !" ;
+            robotPosition =  img.SearchRobot(arena);
+        }
+        rt_mutex_release(&mutex_robot_pos); 
+        
+        //Traitement du robot
+        if(){ // Robot position != null
+            img.DrawAllRobots(robotPosition);
+        } 
+
+        for (auto position : robotPosition) 
+        {
+            WriteInQueue(&q_messageToMon, position); // Envoi de toutes les positions 
         }
                 
         
+
+        // Envoi de l'image
+        WriteInQueue(&q_messageToMon, new MessageImg(MESSAGE_CAM_IMAGE,&img));
+
+        rt_sem_p(&sem_answerSync, TM_INFINITE);
+        rt_mutex_acquire(&mutex_anwer_arena, TM_INFINITE);
+
+        if(arenaConfirm){
+            // Arena OK
+            img.DrawArena(arena); // Comment save ou rejeter? 
+        }else{
+            // Arena not OK
+            // Rien ? 
+        }
+        rt_mutex_release(&mutex_answer_arena);
+        // Je sais pas si cette partie sert a qqc
     }    
 }
 

software/raspberry/superviseur-robot/tasks.h

@@ -67,6 +67,7 @@ private:
     Camera camera;
     int robotStarted = 0;
     int cameraStarted = 0;
+    int arenaConfirm = 0; 
     int move = MESSAGE_ROBOT_STOP;
     
     /**********************************************************************/
@@ -90,6 +91,7 @@ private:
     RT_MUTEX mutex_cameraStarted;
     RT_MUTEX mutex_move;
     RT_MUTEX mutex_camera;
+    RT_MUTEX mutex_answer_arena;
 
     /**********************************************************************/
     /* Semaphores                                                         */
@@ -99,6 +101,7 @@ private:
     RT_SEM sem_serverOk;
     RT_SEM sem_startRobot;
     RT_SEM sem_startCamera;
+    RT_SEM sem_answerSync;
 
     /**********************************************************************/
     /* Message queues                                                     */