From ce68e09679bc118db61eccc521ed12e4d39b61ba Mon Sep 17 00:00:00 2001
From: Robin Marin-Muller <robin.marn.pro@gmail.com>
Date: Wed, 15 May 2024 18:39:08 +0200
Subject: [PATCH] Added basic position management.

---
 .../raspberry/superviseur-robot/tasks.cpp     | 42 ++++++++++++++++++-
 software/raspberry/superviseur-robot/tasks.h  |  9 ++++
 2 files changed, 49 insertions(+), 2 deletions(-)

diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp
index 421a811..3e58ae3 100644
--- a/software/raspberry/superviseur-robot/tasks.cpp
+++ b/software/raspberry/superviseur-robot/tasks.cpp
@@ -17,6 +17,7 @@
 
 #include "tasks.h"
 #include <stdexcept>
+#include <vector>
 
 // Task priority: higher has the best priority 
 #define PRIORITY_TSERVER 30
@@ -35,6 +36,7 @@
 // Their priority is lower.
 #define PRIORITY_TSETCAMERA 18
 #define PRIOTITY_TARENA 17
+#define PRIOTITY_TPOSITION 16
 
 /*
  * Some remarks:
@@ -46,7 +48,7 @@
  * 3- Data flow is probably not optimal
  * 
  * 4- Take into account that ComRobot::Write will block your task when serial buffer is full,
- *   time for internal buffer to flush
+ *   time for interponal buffer to flush
  * 
  * 5- Same behavior existe for ComMonitor::Write !
  * 
@@ -125,6 +127,10 @@ void Tasks::Init() {
         cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_sem_create(&sem_findPosition, NULL, 0, S_FIFO)) {
+        cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
     
     // Added Semaphores
     if (err = rt_sem_create(&sem_manageCamera, NULL, 0, S_FIFO)) {
@@ -408,6 +414,16 @@ void Tasks::ReceiveFromMonTask(void *arg) {
             // Calling arenaChoice, unblocking the thread.
             rt_sem_v(&sem_arenaChoice);
         }
+        else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)){   
+            rt_mutex_acquire(&mutex_positionEnabled, TM_INFINITE);
+            positionEnabled = true;
+            rt_mutex_release(&mutex_positionEnabled);
+        }
+        else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)){   
+            rt_mutex_acquire(&mutex_positionEnabled, TM_INFINITE);
+            positionEnabled = false;
+            rt_mutex_release(&mutex_positionEnabled);
+        }
         else if (msgRcv->CompareID(MESSAGE_CAM_IMAGE)) {
             //?
         }
@@ -634,6 +650,9 @@ void Tasks::ImageCameraTask(void * arg)
     // Variables
     CameraStatusEnum cs = CameraStatusEnum::CLOSED;
     Arena a = Arena();
+    std::vector<MessagePosition*> msgPos = {};
+    MessageImg *msgImg = nullptr;
+    bool pe = false;
     
     cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
     // Synchronization barrier (waiting that all tasks are starting)
@@ -656,6 +675,10 @@ void Tasks::ImageCameraTask(void * arg)
         a = arena;
         rt_mutex_release(&mutex_arena);
         
+        rt_mutex_acquire(&mutex_positionEnabled, TM_INFINITE);
+        pe = positionEnabled;
+        rt_mutex_release(&mutex_positionEnabled);
+        
         // Only gather image if opened
         if(CameraStatusEnum::OPENED == cs)
         {
@@ -668,9 +691,24 @@ void Tasks::ImageCameraTask(void * arg)
             if(!a.IsEmpty())
                 img->DrawArena(a);
             
+            // Check position
+            auto listPos = img->SearchRobot(a);
+            img->DrawAllRobots(listPos);
+            
             // Sending the image to the monitor
-            MessageImg *msgImg = new MessageImg(MESSAGE_CAM_IMAGE, img);
+            if(!listPos.empty())
+            {
+                for(auto l : listPos)
+                    msgPos.emplace_back(new MessagePosition(MESSAGE_CAM_POSITION, l));
+            }
+            msgImg= new MessageImg(MESSAGE_CAM_IMAGE, img);
             rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
+            if(!listPos.empty())
+            {
+                for(auto mg : msgPos)
+                    monitor.Write(mg);
+                msgPos.clear();
+            }
             monitor.Write(msgImg);
             rt_mutex_release(&mutex_monitor);
         }
diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h
index 00f02dc..d73a22f 100644
--- a/software/raspberry/superviseur-robot/tasks.h
+++ b/software/raspberry/superviseur-robot/tasks.h
@@ -108,6 +108,8 @@ private:
     ArenaStatusEnum arenaStatus = ArenaStatusEnum::NONE;
     // arena object (overlay of the arena on the img)
     Arena arena;
+    // when gathering position, set to true
+    bool positionEnabled = false;
 
     /**********************************************************************/
     /* Tasks                                                              */
@@ -143,6 +145,8 @@ private:
     RT_MUTEX mutex_arena;
     // Locking the variable 'arenaStatus'
     RT_MUTEX mutex_arenaStatus;
+    // Locking the variable 'positionEnabled'
+    RT_MUTEX mutex_positionEnabled;
 
     /**********************************************************************/
     /* Semaphores                                                         */
@@ -158,6 +162,8 @@ private:
     RT_SEM sem_manageCamera;
     // Used to call ArenaChoiceTask (non-periodic)
     RT_SEM sem_arenaChoice;
+    // Used to call FindPositionTask (non-periodic)
+    RT_SEM sem_findPosition;
 
     /**********************************************************************/
     /* Message queues                                                     */
@@ -230,6 +236,9 @@ private:
      */
     void ArenaChoiceTask(void * arg);
     
+    
+    void FindPositionTask(void * arg);
+    
     /************************************************************************/
     /* Added functions                                                      */
     /************************************************************************/