From d89d36d0401fff74753973865189d5cb6d704389 Mon Sep 17 00:00:00 2001 From: Robin Marin-Muller Date: Wed, 15 May 2024 19:37:20 +0200 Subject: [PATCH] Fully working positions with multiple robots --- .../raspberry/superviseur-robot/tasks.cpp | 29 +++++++++++-------- 1 file changed, 17 insertions(+), 12 deletions(-) diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 3e58ae3..bcb3c1e 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -653,6 +653,7 @@ void Tasks::ImageCameraTask(void * arg) std::vector msgPos = {}; MessageImg *msgImg = nullptr; bool pe = false; + std::list listPos = {}; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are starting) @@ -691,26 +692,30 @@ 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 - if(!listPos.empty()) + // Gathering position + if(pe) { + // Gather every robot available + listPos = img->SearchRobot(a); + // For every position, draw the robot and send the message for(auto l : listPos) + { + img->DrawRobot(l); 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(); - } + // Writing every message position + for(auto mp : msgPos) + monitor.Write(mp); + // Writing every message image monitor.Write(msgImg); rt_mutex_release(&mutex_monitor); + + // clearing the vector because all its values are unassigned + msgPos.clear(); } } }