vision funciton not fully working but almost

This commit is contained in:
Romain Vitrat 2020-03-29 11:19:22 +02:00
parent 481213ac20
commit be868ed6f4
2 changed files with 44 additions and 20 deletions

View file

@ -154,7 +154,10 @@ void Tasks::Init() {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush; cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
if (err = rt_sem_create(&sem_allowStartCapture, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
//Initialization for some specific sem: Allow to run the first time //Initialization for some specific sem: Allow to run the first time
rt_sem_v(&sem_allowStartReceive); rt_sem_v(&sem_allowStartReceive);
@ -668,76 +671,96 @@ void Tasks::Vision(void *arg){
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting) // Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE); rt_sem_p(&sem_barrier, TM_INFINITE);
rt_sem_p(&sem_askBattery, TM_INFINITE);
int killVisionOk=0; int killVisionOk=0;
Camera camera; Camera camera;
int acquireImageOk=1; int acquireImageOk=0;
int searchArenaOk=0; int searchArenaOk=0;
int drawArenaOk=0; int drawArenaOk=0;
int getPositionOk=0; int getPositionOk=0;
Arena arena; Arena arena;
//Img* img;
list<Position> positionList; list<Position> positionList;
list<Position>::iterator it; list<Position>::iterator it;
string strPos; string strPos;
MessagePosition* msgPos; //MessagePosition* msgPos;
Message* msgPosMsg; Message* msgPosMsg;
MessageImg* msgImg; MessageImg msgImg;
Message* msgImgMsg; Message* msgImgMsg;
msgPos->SetID(MESSAGE_CAM_POSITION); msgImg.SetID(MESSAGE_CAM_IMAGE);
msgImg->SetID(MESSAGE_CAM_IMAGE); //msgPos->SetID(MESSAGE_CAM_POSITION);
rt_mutex_acquire(&mutex_killVision, TM_INFINITE); rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
killVision=0; killVision=0;
rt_mutex_release(&mutex_killVision); rt_mutex_release(&mutex_killVision);
camera.Open(); rt_task_set_periodic(NULL, TM_NOW, 100000000);
camera.Open();
while(killVisionOk==0){ while(killVisionOk==0){
rt_task_wait_period(NULL);
while(acquireImageOk==1){ while(acquireImageOk==1){
rt_task_wait_period(NULL);
cout << "open cam" << endl;
Img img=camera.Grab(); Img img=camera.Grab();
cout << "after camera grab" << endl << endl << endl;
msgImg.SetImage(&img);
cout << "after set image in msgImg" << endl << endl << endl;
if(searchArenaOk==1){ if(searchArenaOk==1){
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
acquireImage=0; acquireImage=0;
rt_mutex_release(&mutex_acquireImage); rt_mutex_release(&mutex_acquireImage);
acquireImageOk=acquireImage;
arena=img.SearchArena(); arena=img.SearchArena();
img.DrawArena(arena); img.DrawArena(arena);
} }
if(drawArenaOk==1){ if(drawArenaOk==1){
// img.DrawArena(arena); img.DrawArena(arena);
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
acquireImage=1; acquireImage=1;
rt_mutex_release(&mutex_acquireImage); rt_mutex_release(&mutex_acquireImage);
acquireImageOk=acquireImage;
} }
if(getPositionOk==1){ if(getPositionOk==1){
cout << "in GetPos=1" << endl << endl << endl;
//On démarre la recherche du robot dans la zone définie par l'arène //On démarre la recherche du robot dans la zone définie par l'arène
positionList=img.SearchRobot(arena); positionList=img.SearchRobot(arena);
//Définitition et assignation de l'itérateur de parcrous de la liste positionList //Définitition et assignation de l'itérateur de parcrous de la liste positionList
it=positionList.begin(); it=positionList.begin();
//Définition d'un messagePosition qui va contenir l'information (x,y) //Définition d'un messagePosition qui va contenir l'information (x,y)
msgPos->SetPosition(*it); //msgPos->SetPosition(*it);
MessagePosition msgPos(MESSAGE_CAM_POSITION,*it);
//Transformation en message classique //Transformation en message classique
msgPosMsg=msgPos->Copy(); msgPosMsg=msgPos.Copy();
//Envoi //Envoi
WriteInQueue(&q_messageToMon,msgPos); cout << "envoi au mon" << endl;
WriteInQueue(&q_messageToMon,&msgPos);
//Dessis du robot sur l'image //Dessis du robot sur l'image
img.DrawRobot(*it); img.DrawRobot(*it);
} }
//Définition d'un messageImg contenant l'image avec le robot et l'arène dessinés (ou pas) //Définition d'un messageImg contenant l'image avec le robot et l'arène dessinés (ou pas)
msgImg->SetImage(&img); msgImg.SetImage(&img);
//Transformation en message classique //Transformation en message classique
msgImgMsg=msgImg->Copy(); msgImgMsg=msgImg.Copy();
//Envoi //Envoi
WriteInQueue(&q_messageToMon,msgImg); WriteInQueue(&q_messageToMon,&msgImg);
} }
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
acquireImageOk=acquireImage;
rt_mutex_release(&mutex_acquireImage);
rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
getPositionOk=getPosition;
rt_mutex_release(&mutex_getPosition);
rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
searchArenaOk=searchArena;
rt_mutex_release(&mutex_searchArena);
rt_mutex_acquire(&mutex_drawArena, TM_INFINITE);
drawArenaOk=drawArena;
rt_mutex_release(&mutex_drawArena);
} }
rt_mutex_acquire(&mutex_searchArena, TM_INFINITE); rt_mutex_acquire(&mutex_searchArena, TM_INFINITE);
searchArena=0; searchArena=0;
rt_mutex_release(&mutex_searchArena); rt_mutex_release(&mutex_searchArena);
searchArenaOk=1; searchArenaOk=0;
rt_mutex_acquire(&mutex_getPosition, TM_INFINITE); rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
getPosition=0; getPosition=0;
rt_mutex_release(&mutex_getPosition); rt_mutex_release(&mutex_getPosition);
getPositionOk=1; getPositionOk=0;
} }

View file

@ -117,6 +117,7 @@ private:
RT_SEM sem_startRobotWithWatchdog; RT_SEM sem_startRobotWithWatchdog;
RT_SEM sem_allowStartReceive; RT_SEM sem_allowStartReceive;
RT_SEM sem_askBattery; RT_SEM sem_askBattery;
RT_SEM sem_allowStartCapture;
/**********************************************************************/ /**********************************************************************/
/* Message queues */ /* Message queues */