From 670fff6a0aef6ce3a9869cca50dc30506fb3171c Mon Sep 17 00:00:00 2001 From: alejeune Date: Fri, 7 Apr 2023 12:10:04 +0200 Subject: [PATCH] Camera almost finished: todo suppress tooggle, semaphore battery, fps count --- .../raspberry/superviseur-robot/lib/img.h | 2 +- .../nbproject/private/configurations.xml | 2 +- .../private/downloads-10.105.1.3-xenomai-22 | 0 .../nbproject/private/private.xml | 2 + .../private/timestamps-10.105.1.3-xenomai-22 | 39 +++++++++++ .../raspberry/superviseur-robot/tasks.cpp | 68 ++++++++++++------- software/raspberry/superviseur-robot/tasks.h | 2 +- 7 files changed, 88 insertions(+), 27 deletions(-) create mode 100644 software/raspberry/superviseur-robot/nbproject/private/downloads-10.105.1.3-xenomai-22 create mode 100644 software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.3-xenomai-22 diff --git a/software/raspberry/superviseur-robot/lib/img.h b/software/raspberry/superviseur-robot/lib/img.h index 11822e0..191d5c6 100644 --- a/software/raspberry/superviseur-robot/lib/img.h +++ b/software/raspberry/superviseur-robot/lib/img.h @@ -39,7 +39,7 @@ using namespace std; /** * Redefinition of cv::Mat type */ -typedef cv::Mat ImageMat; + typedef cv::Mat ImageMat; /** * Declaration of Jpg type diff --git a/software/raspberry/superviseur-robot/nbproject/private/configurations.xml b/software/raspberry/superviseur-robot/nbproject/private/configurations.xml index ffe1427..a20af05 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/configurations.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/configurations.xml @@ -41,7 +41,7 @@ - xenomai@10.105.1.8:22 + xenomai@10.105.1.3:22 2 diff --git a/software/raspberry/superviseur-robot/nbproject/private/downloads-10.105.1.3-xenomai-22 b/software/raspberry/superviseur-robot/nbproject/private/downloads-10.105.1.3-xenomai-22 new file mode 100644 index 0000000..e69de29 diff --git a/software/raspberry/superviseur-robot/nbproject/private/private.xml b/software/raspberry/superviseur-robot/nbproject/private/private.xml index 307d1c7..56849c0 100644 --- a/software/raspberry/superviseur-robot/nbproject/private/private.xml +++ b/software/raspberry/superviseur-robot/nbproject/private/private.xml @@ -9,8 +9,10 @@ file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.h file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.h + file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/img.cpp file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.cpp file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.cpp + file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.cpp file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/main.cpp file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.h diff --git a/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.3-xenomai-22 b/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.3-xenomai-22 new file mode 100644 index 0000000..536a776 --- /dev/null +++ b/software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.3-xenomai-22 @@ -0,0 +1,39 @@ +#Fri Apr 07 11:52:43 CEST 2023 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Package-Debug__RPI_.bash=c1679401432930 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.cpp=c1680248859421 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp=c1679401432864 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/gdbsudo.sh=c1679401432813 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/LICENSE=c1679401432832 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp=c1679401457314 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.cpp=c1680861150700 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/.dep.inc=c1679401432701 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk=c1679401432922 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/img.cpp=c1679401457320 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.h=c1680518630237 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/README.md=c1679401432714 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__PC_.mk=c1679401457337 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/Makefile=c1679401432709 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/.gitignore=c1679401432820 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/wrapper.c=c1679401457381 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.h=c1680860665596 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/main.cpp=c1679404095224 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/project.properties=c1679401432983 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.cpp=c1679401457304 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/test.cpp=c1679401432851 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/base64.h=c1679401432844 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk=c1679401457346 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1679401432926 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/project.xml=c1679401457377 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/.gitignore=c1679401432827 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1679401432918 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk=c1679401432944 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/superviseur.doxygen=c1679401432993 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.h=c1679401432897 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/base64.cpp=c1679401432840 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/comrobot.h=c1679401432877 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/img.h=c1680855564574 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/.gitignore=c1679401432706 +VERSION=1.3 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/commonitor.h=c1679401432868 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/README.md=c1679401432836 +/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/compile-and-run-test=c1679401432848 diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 1223fb8..dab0eea 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -22,12 +22,12 @@ #define PRIORITY_TSERVER 30 #define PRIORITY_TOPENCOMROBOT 20 #define PRIORITY_TMOVE 20 -#define PRIORITY_TSENDTOMON 22 +#define PRIORITY_TSENDTOMON 23 #define PRIORITY_TRECEIVEFROMMON 25 #define PRIORITY_TSTARTROBOT 20 #define PRIORITY_TCAMERA 21 -#define PRIORITY_TBATTERY 25 -#define PRIORITY_TSTARTCAMERA 25 +#define PRIORITY_TBATTERY 19 +#define PRIORITY_TSTARTCAMERA 22 /* @@ -46,7 +46,19 @@ * * 6- When you want to write something in terminal, use cout and terminate with endl and flush * - * 7- Good luck ! + * 7- Good luck !#include + +// Déclaration des priorités des taches +#define PRIORITY_TSERVER 30 +#define PRIORITY_TOPENCOMROBOT 20 +#define PRIORITY_TMOVE 20 +#define PRIORITY_TSENDTOMON 23 +#define PRIORITY_TRECEIVEFROMMON 25 +#define PRIORITY_TSTARTROBOT 20 +#define PRIORITY_TCAMERA 21 +#define PRIORITY_TBATTERY 19 +#define PRIORITY_TSTARTCAMERA 22 + */ /** @@ -164,12 +176,14 @@ void Tasks::Init() { /**************************************************************************************/ /* Message queues creation */ /**************************************************************************************/ - if ((err = rt_queue_create(&q_messageToMon, "q_messageToMon", sizeof (Message*)*50, Q_UNLIMITED, Q_FIFO)) < 0) { + if ((err = rt_queue_create(&q_messageToMon, "q_messageToMon", sizeof (Message*)*100, Q_UNLIMITED, Q_FIFO)) < 0) { cerr << "Error msg queue create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } cout << "Queues created successfully" << endl << flush; - + + camera = new Camera(sm,5); + cout << "New camera ! " << endl << flush; } /** @@ -329,7 +343,7 @@ void Tasks::ReceiveFromMonTask(void *arg) { } else if(msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) || msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)){ rt_mutex_acquire(&mutex_answer_arena,TM_INFINITE); arenaConfirm = msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)? 1:0; // A vérifier que le ternaire est utile - cout << "Staut de l'arene" << arenaConfirm << endl << flush; + cout << "Statut de l'arene" << arenaConfirm << endl << flush; rt_mutex_release(&mutex_answer_arena); rt_sem_v(&sem_answerSync); }else if(msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)){ @@ -439,10 +453,10 @@ void Tasks::StartCameraTask(void *arg) { rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE); rt_mutex_acquire(&mutex_camera, TM_INFINITE); if(!cameraStarted){ - status = camera.Open(); + status = camera->Open(); cout << "Camera Opened" << endl << flush; }else{ - camera.Close(); + camera->Close(); status = true; cout << "Camera Closed" << endl << flush; } @@ -520,7 +534,7 @@ void Tasks::BatteryTask(void *arg) { /**************************************************************************************/ rt_task_set_periodic(NULL, TM_NOW, 500*1000*1000); - while (1) { + /*while (1) { rt_task_wait_period(NULL); rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rs = robotStarted; @@ -534,7 +548,7 @@ void Tasks::BatteryTask(void *arg) { } rt_mutex_release(&mutex_robotStarted); - } + }*/ } /** @@ -542,7 +556,11 @@ void Tasks::BatteryTask(void *arg) { */ void Tasks::CameraTask(void *arg){ int cam; + Img* image; + Arena arenaSaved ; std::list robotPosition; + MessageImg* imgToSend; + cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); @@ -557,12 +575,15 @@ void Tasks::CameraTask(void *arg){ continue; } + cout << "Traitement de l'image" << endl << flush ; 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 + + Img img = camera->Grab(); + image = img.Copy(); rt_mutex_release(&mutex_camera); rt_mutex_acquire(&mutex_search_arena, TM_INFINITE); + Arena arena = image->SearchArena(); // Mise a jour de l'arene tout le temps pour la position du robot mais draw si search arena = true if(searchArena){ // chopper l'arene if(arena.IsEmpty()){ @@ -571,23 +592,22 @@ void Tasks::CameraTask(void *arg){ }else{ cout << "Draw Arena" << endl << flush; - img.DrawArena(arena); - + image->DrawArena(arena); // envoi de l'arene pour vérif } + }else { + image->DrawArena(arenaSaved); } rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); if(robotPos){ // chopper le robot cout << "Pos du robot pls !" ; - robotPosition = img.SearchRobot(arena); - - + robotPosition = image->SearchRobot(arena); //Traitement du robot if(!robotPosition.empty()){ // Robot position != null - img.DrawAllRobots(robotPosition); + image->DrawAllRobots(robotPosition); } for (auto position : robotPosition) { @@ -597,23 +617,23 @@ void Tasks::CameraTask(void *arg){ } rt_mutex_release(&mutex_robot_pos); // Envoi de l'image - WriteInQueue(&q_messageToMon, new MessageImg(MESSAGE_CAM_IMAGE,&img)); + cout << "Envoi de l'image" << endl << flush ; + imgToSend = new MessageImg(MESSAGE_CAM_IMAGE,image); + WriteInQueue(&q_messageToMon, imgToSend); rt_sem_p(&sem_answerSync, TM_INFINITE); rt_mutex_acquire(&mutex_answer_arena, TM_INFINITE); if(arenaConfirm){ // Arena OK - img.DrawArena(arena); // Comment save ou rejeter? - //searchArena = 0; + arenaSaved = arena; + searchArena = 0; }else{ // Arena not OK - searchArena = 0; // Rien ? } rt_mutex_release(&mutex_answer_arena); rt_mutex_release(&mutex_search_arena); - // Je sais pas si cette partie sert a qqc } } diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h index a2216f2..b89b7d8 100644 --- a/software/raspberry/superviseur-robot/tasks.h +++ b/software/raspberry/superviseur-robot/tasks.h @@ -64,7 +64,7 @@ private: /**********************************************************************/ ComMonitor monitor; ComRobot robot; - Camera camera; + Camera* camera; int robotStarted = 0; int robotPos = 0; int cameraStarted = 0;