Final Project and Vision
This commit is contained in:
parent
2cd32fe997
commit
0027f2e4f9
2 changed files with 83 additions and 77 deletions
|
|
@ -157,6 +157,11 @@ void Tasks::Init() {
|
|||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
if (err = rt_sem_create(&sem_startVision, NULL, 0, S_FIFO)) {
|
||||
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
cout << "Semaphores created successfully" << endl << flush;
|
||||
|
||||
/**************************************************************************************/
|
||||
|
|
@ -454,6 +459,7 @@ void Tasks::ReceiveFromMonTask(void *arg) {
|
|||
} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
|
||||
cout << "Command Open Camera Received" << endl << flush;
|
||||
//start task Vision
|
||||
rt_sem_v(&sem_startVision);
|
||||
|
||||
} else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) {
|
||||
cout << "Command Close Camera Received" << endl << flush;
|
||||
|
|
@ -848,15 +854,7 @@ void Tasks::AskBattery(void *arg){
|
|||
|
||||
|
||||
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);
|
||||
int killVisionOk=0;
|
||||
Camera camera;
|
||||
int acquireImageOk=0;
|
||||
int searchArenaOk=0;
|
||||
int drawArenaOk=0;
|
||||
int getPositionOk=0;
|
||||
Arena arena;
|
||||
//Img* img;
|
||||
list<Position> positionList;
|
||||
|
|
@ -867,80 +865,87 @@ void Tasks::Vision(void *arg){
|
|||
MessageImg msgImg;
|
||||
Message* msgImgMsg;
|
||||
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){
|
||||
|
||||
// Synchronization barrier (waiting that all tasks are starting)
|
||||
rt_sem_p(&sem_barrier, TM_INFINITE);
|
||||
|
||||
while(1){
|
||||
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
|
||||
//Wait to get the order to start Vision managed in receiveFromMon
|
||||
rt_sem_p(&sem_startVision, TM_INFINITE);
|
||||
|
||||
int killVisionOk=0;
|
||||
int acquireImageOk=0;
|
||||
int searchArenaOk=0;
|
||||
int drawArenaOk=0;
|
||||
int getPositionOk=0;
|
||||
//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);
|
||||
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);
|
||||
arena=img.SearchArena();
|
||||
img.DrawArena(arena);
|
||||
}
|
||||
if(drawArenaOk==1){
|
||||
img.DrawArena(arena);
|
||||
rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE);
|
||||
acquireImage=1;
|
||||
rt_mutex_release(&mutex_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);
|
||||
MessagePosition msgPos(MESSAGE_CAM_POSITION,*it);
|
||||
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);
|
||||
arena=img.SearchArena();
|
||||
img.DrawArena(arena);
|
||||
}
|
||||
if(drawArenaOk==1){
|
||||
img.DrawArena(arena);
|
||||
}
|
||||
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);
|
||||
MessagePosition msgPos(MESSAGE_CAM_POSITION,*it);
|
||||
//Transformation en message classique
|
||||
msgPosMsg=msgPos.Copy();
|
||||
//Envoi
|
||||
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);
|
||||
//Transformation en message classique
|
||||
msgPosMsg=msgPos.Copy();
|
||||
msgImgMsg=msgImg.Copy();
|
||||
//Envoi
|
||||
cout << "envoi au mon" << endl;
|
||||
WriteInQueue(&q_messageToMon,&msgPos);
|
||||
//Dessis du robot sur l'image
|
||||
img.DrawRobot(*it);
|
||||
WriteInQueue(&q_messageToMon,&msgImg);
|
||||
}
|
||||
|
||||
//Définition d'un messageImg contenant l'image avec le robot et l'arène dessinés (ou pas)
|
||||
msgImg.SetImage(&img);
|
||||
//Transformation en message classique
|
||||
msgImgMsg=msgImg.Copy();
|
||||
//Envoi
|
||||
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_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=0;
|
||||
rt_mutex_acquire(&mutex_getPosition, TM_INFINITE);
|
||||
getPosition=0;
|
||||
rt_mutex_release(&mutex_getPosition);
|
||||
getPositionOk=0;
|
||||
rt_mutex_acquire(&mutex_killVision, TM_INFINITE);
|
||||
killVisionOk = killVision;
|
||||
rt_mutex_release(&mutex_killVision);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -121,6 +121,7 @@ private:
|
|||
RT_SEM sem_startRobotWithWatchdog;
|
||||
RT_SEM sem_detectLostSupRob;
|
||||
RT_SEM sem_askBattery;
|
||||
RT_SEM sem_startVision;
|
||||
|
||||
/**********************************************************************/
|
||||
/* Message queues */
|
||||
|
|
|
|||
Loading…
Reference in a new issue