Camera ouverte traitement en cours

This commit is contained in:
alejeune 2023-03-31 12:13:03 +02:00
parent 2981e6b0a4
commit 694c6314a3
6 changed files with 130 additions and 41 deletions

View file

@ -52,7 +52,7 @@ public:
/**
* Open camera
* @return True if camera is open, false otherwise
* @return True if camera is open, fallocalhostse otherwise
*/
bool Open();
@ -62,7 +62,7 @@ public:
void Close();
/**
* Get width of sampled image
* Get width of sampled imagelocalhost
* @return Width of sampled picture
*/
int GetWidth() const;

View file

@ -41,7 +41,7 @@
</conf>
<conf name="Debug__RPI_" type="1">
<toolsSet>
<developmentServer>xenomai@10.105.1.5:22</developmentServer>
<developmentServer>xenomai@10.105.1.10:22</developmentServer>
<platform>2</platform>
</toolsSet>
<dbx_gdbdebugger version="1">

View file

@ -6,6 +6,11 @@
</data>
<editor-bookmarks xmlns="http://www.netbeans.org/ns/editor-bookmarks/2" lastBookmarkId="0"/>
<open-files xmlns="http://www.netbeans.org/ns/projectui-open-files/2">
<group/>
<group>
<file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.h</file>
<file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.h</file>
<file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.cpp</file>
<file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/main.cpp</file>
</group>
</open-files>
</project-private>

View file

@ -1,20 +1,20 @@
#Tue Mar 21 15:11:16 CET 2023
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.cpp=c1679401432892
#Fri Mar 31 12:07:07 CEST 2023
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.cpp=c1680248859421
/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/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=c1679407831774
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.cpp=c1680257220969
/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=c1679401457309
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.h=c1680256228274
/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/tasks.h=c1679406345632
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.h=c1680253954166
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/wrapper.c=c1679401457381
/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

View file

@ -27,6 +27,7 @@
#define PRIORITY_TSTARTROBOT 20
#define PRIORITY_TCAMERA 21
#define PRIORITY_TBATTERY 25
#define PRIORITY_TSTARTCAMERA 25
/*
* Some remarks:
@ -74,6 +75,22 @@ void Tasks::Init() {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_cameraStarted, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_robot_pos, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_search_arena, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_camera, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Mutexes created successfully" << endl << flush;
/**************************************************************************************/
@ -95,6 +112,10 @@ void Tasks::Init() {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&sem_startCamera, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Semaphores created successfully" << endl << flush;
/**************************************************************************************/
@ -120,6 +141,10 @@ void Tasks::Init() {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_startCamera, "th_startCamera", 0, PRIORITY_TSTARTCAMERA, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
@ -128,6 +153,10 @@ void Tasks::Init() {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_camera, "th_camera", 0, PRIORITY_TCAMERA, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Tasks created successfully" << endl << flush;
/**************************************************************************************/
@ -176,7 +205,15 @@ void Tasks::Run() {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_camera, (void(*)(void*)) & Tasks::CameraTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_startCamera, (void(*)(void*)) & Tasks::StartCameraTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Tasks launched" << endl << flush;
}
@ -285,6 +322,18 @@ 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_ASK_ARENA)){
rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
searchArena = 1;
rt_mutex_release(&mutex_search_arena);
}else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)){
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
robotPos = 1;
rt_mutex_release(&mutex_robot_pos);
}else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)){
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
robotPos = 0;
rt_mutex_release(&mutex_robot_pos);
}
delete(msgRcv); // must be deleted manually, no consumer
}
@ -370,25 +419,34 @@ void Tasks::StartCameraTask(void *arg) {
while (1) {
Message * msgSend;
Message * msg;
rt_sem_p(&sem_startCamera, TM_INFINITE);
cout << "Start Camera ";
rt_mutex_acquire(&mutex_camera, TM_INFINITE);
if(!camera.Open()){
msgSend = new Message(MESSAGE_ANSWER_NACK)
bool status = camera.Open();
if(!status){
msgSend = new Message(MESSAGE_ANSWER_NACK);
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
cout << msgSend->GetID << endl << flush;
cout << "NACK !" << endl << flush ;
cout << msgSend->ToString() << endl << flush;
cout << "NACK1 ! " << endl << flush ;
rt_mutex_release(&mutex_camera);
cout << "NACK2 ! " << endl << flush ;
continue;
}
msg = new Message(MESSAGE_ANSWER_ACK);
cout << msgSend->GetID << endl << flush;
msg = new Message(MESSAGE_ANSWER_ACK);
cout << "ACK3 ! " << endl << flush ;
cout << msg->ToString() << endl << flush;
cout << "ACK4 ! " << endl << flush ;
rt_mutex_release(&mutex_camera);
if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
cameraStarted = 1;
rt_mutex_release(&mutex_cameraStarted);
}
cout << "ACK ! " << endl << flush ;
rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
cout << "ff" << endl << flush ;
cameraStarted = 1;
cout << "Cam success! " << endl <<flush;
rt_mutex_release(&mutex_cameraStarted);
cout << "Fin de start cam" << endl << flush;
}
}
@ -403,14 +461,8 @@ void Tasks::MoveTask(void *arg) {
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
/**************************************************************************************/
/* The task starts here */
/**************************************************************************************/
rt_task_set_periodic(NULL, TM_NOW, 100000000);
while (1) {
rt_task_wait_period(NULL);
cout << "Periodic movement update";
/*********************************************************/
//cout << "Periodic movement update";
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
rs = robotStarted;
if (rs == 1) {
@ -425,9 +477,8 @@ void Tasks::MoveTask(void *arg) {
rt_mutex_release(&mutex_robot);
}
rt_mutex_release(&mutex_robotStarted);
cout << endl << flush;
//cout << endl << flush;
}
}
/**
* @brief task in charge of the battery status.
@ -462,10 +513,12 @@ void Tasks::BatteryTask(void *arg) {
}
}
/**
* @brief task in charge of managing the camera.
*/
void CameraTask(void *arg){
void Tasks::CameraTask(void *arg){
int cam;
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
@ -473,19 +526,46 @@ void CameraTask(void *arg){
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){
// Lancer le traitement périodique
rt_mutex_release(&mutex_camera);
}else{
rt_mutex_release(&mutex_camera);
rt_mutex_acquire(&mutex, TM_INFINITE);
rt_mutex_release(&mutex_camera);
continue;
}
rt_mutex_acquire(&mutex_camera, TM_INFINITE);
// Lancer le traitement périodique
Img img = camera.Grab();
Arena arena;
rt_mutex_release(&mutex_camera);
rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
if(searchArena){
// chopper l'arene
cout << "Search arena pls ! " << endl << flush;
arena = img.SearchArena();
// searchArena = 0; // toggle?
}
rt_mutex_release(&mutex_search_arena);
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
if(robotPos){
// chopper le robot
cout << "Pos du robot pls !" ;
}
rt_mutex_release(&mutex_robot_pos);
if(arena.IsEmpty()){
cout << "Arena empty" << endl << flush;
// envoi d'un no ack au moniteur
}else{
cout << "Draw Arena" << endl << flush;
img.DrawArena(arena);
// envoi de l'arene pour vérif
}
cout << "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAARENE" << endl << flush;
WriteInQueue(&q_messageToMon, new MessageImg(MESSAGE_CAM_IMAGE,&img));
}
}

View file

@ -67,6 +67,8 @@ private:
Camera camera;
int robotStarted = 0;
int cameraStarted = 0;
int searchArena = 0;
int robotPos = 0;
int move = MESSAGE_ROBOT_STOP;
/**********************************************************************/
@ -90,6 +92,8 @@ private:
RT_MUTEX mutex_cameraStarted;
RT_MUTEX mutex_move;
RT_MUTEX mutex_camera;
RT_MUTEX mutex_search_arena;
RT_MUTEX mutex_robot_pos;
/**********************************************************************/
/* Semaphores */