vision funciton not fully working but almost
This commit is contained in:
parent
481213ac20
commit
be868ed6f4
2 changed files with 44 additions and 20 deletions
|
@ -154,7 +154,10 @@ void Tasks::Init() {
|
|||
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
|
||||
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
|
||||
rt_sem_v(&sem_allowStartReceive);
|
||||
|
@ -668,76 +671,96 @@ void Tasks::Vision(void *arg){
|
|||
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
|
||||
// Synchronization barrier (waiting that all tasks are starting)
|
||||
rt_sem_p(&sem_barrier, TM_INFINITE);
|
||||
|
||||
rt_sem_p(&sem_askBattery, TM_INFINITE);
|
||||
int killVisionOk=0;
|
||||
Camera camera;
|
||||
int acquireImageOk=1;
|
||||
int acquireImageOk=0;
|
||||
int searchArenaOk=0;
|
||||
int drawArenaOk=0;
|
||||
int getPositionOk=0;
|
||||
Arena arena;
|
||||
//Img* img;
|
||||
list<Position> positionList;
|
||||
list<Position>::iterator it;
|
||||
string strPos;
|
||||
MessagePosition* msgPos;
|
||||
//MessagePosition* msgPos;
|
||||
Message* msgPosMsg;
|
||||
MessageImg* msgImg;
|
||||
MessageImg msgImg;
|
||||
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);
|
||||
killVision=0;
|
||||
rt_mutex_release(&mutex_killVision);
|
||||
rt_task_set_periodic(NULL, TM_NOW, 100000000);
|
||||
camera.Open();
|
||||
while(killVisionOk==0){
|
||||
rt_task_wait_period(NULL);
|
||||
while(acquireImageOk==1){
|
||||
rt_task_wait_period(NULL);
|
||||
cout << "open cam" << endl;
|
||||
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){
|
||||
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
|
||||
acquireImage=0;
|
||||
rt_mutex_release(&mutex_acquireImage);
|
||||
acquireImageOk=acquireImage;
|
||||
arena=img.SearchArena();
|
||||
img.DrawArena(arena);
|
||||
}
|
||||
if(drawArenaOk==1){
|
||||
// img.DrawArena(arena);
|
||||
img.DrawArena(arena);
|
||||
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
|
||||
acquireImage=1;
|
||||
rt_mutex_release(&mutex_acquireImage);
|
||||
acquireImageOk=acquireImage;
|
||||
}
|
||||
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
|
||||
positionList=img.SearchRobot(arena);
|
||||
//Définitition et assignation de l'itérateur de parcrous de la liste positionList
|
||||
it=positionList.begin();
|
||||
//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
|
||||
msgPosMsg=msgPos->Copy();
|
||||
msgPosMsg=msgPos.Copy();
|
||||
//Envoi
|
||||
WriteInQueue(&q_messageToMon,msgPos);
|
||||
cout << "envoi au mon" << endl;
|
||||
WriteInQueue(&q_messageToMon,&msgPos);
|
||||
//Dessis du robot sur l'image
|
||||
img.DrawRobot(*it);
|
||||
}
|
||||
|
||||
//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
|
||||
msgImgMsg=msgImg->Copy();
|
||||
msgImgMsg=msgImg.Copy();
|
||||
//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);
|
||||
searchArena=0;
|
||||
rt_mutex_release(&mutex_searchArena);
|
||||
searchArenaOk=1;
|
||||
searchArenaOk=0;
|
||||
rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
|
||||
getPosition=0;
|
||||
rt_mutex_release(&mutex_getPosition);
|
||||
getPositionOk=1;
|
||||
getPositionOk=0;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -117,6 +117,7 @@ private:
|
|||
RT_SEM sem_startRobotWithWatchdog;
|
||||
RT_SEM sem_allowStartReceive;
|
||||
RT_SEM sem_askBattery;
|
||||
RT_SEM sem_allowStartCapture;
|
||||
|
||||
/**********************************************************************/
|
||||
/* Message queues */
|
||||
|
|
Loading…
Reference in a new issue