|
@@ -193,7 +193,7 @@ void Tasks::Run() {
|
193
|
193
|
cerr << "Error task start: " << strerror(-err) << endl << flush;
|
194
|
194
|
exit(EXIT_FAILURE);
|
195
|
195
|
}
|
196
|
|
- if (err = rt_task_start(&th_startRobot, (void(*)(void*)) & Tasks::StartRobotTask, this)) {
|
|
196
|
+ if (err = rt_task_start(&th_startRobot, (void(*)(void*)) & Tasks::StartRobotTask, this)) {
|
197
|
197
|
cerr << "Error task start: " << strerror(-err) << endl << flush;
|
198
|
198
|
exit(EXIT_FAILURE);
|
199
|
199
|
}
|
|
@@ -205,7 +205,15 @@ void Tasks::Run() {
|
205
|
205
|
cerr << "Error task start: " << strerror(-err) << endl << flush;
|
206
|
206
|
exit(EXIT_FAILURE);
|
207
|
207
|
}
|
208
|
|
- if (err = rt_task_start(&th_watchdog, (void(*)(void*)) & Tasks::ReloadWatchdogTask, this)) {
|
|
208
|
+ if (err = rt_task_start(&th_startCamera, (void(*)(void*)) & Tasks::StartCamera, this)) {
|
|
209
|
+ cerr << "Error task start: " << strerror(-err) << endl << flush;
|
|
210
|
+ exit(EXIT_FAILURE);
|
|
211
|
+ }
|
|
212
|
+ if (err = rt_task_start(&th_watchdog, (void(*)(void*)) & Tasks::ReloadWatchdogTask, this)) {
|
|
213
|
+ cerr << "Error task start: " << strerror(-err) << endl << flush;
|
|
214
|
+ exit(EXIT_FAILURE);
|
|
215
|
+ }
|
|
216
|
+ if (err = rt_task_start(&th_camera, (void(*)(void*)) & Tasks::CameraTask, this)) {
|
209
|
217
|
cerr << "Error task start: " << strerror(-err) << endl << flush;
|
210
|
218
|
exit(EXIT_FAILURE);
|
211
|
219
|
}
|
|
@@ -217,6 +225,7 @@ void Tasks::Run() {
|
217
|
225
|
* @brief Arrêt des tâches
|
218
|
226
|
*/
|
219
|
227
|
void Tasks::Stop() {
|
|
228
|
+ camera.Close();
|
220
|
229
|
monitor.Close();
|
221
|
230
|
robot.Close();
|
222
|
231
|
}
|
|
@@ -320,7 +329,14 @@ void Tasks::ReceiveFromMonTask(void *arg) {
|
320
|
329
|
rt_mutex_acquire(&mutex_watchdog, TM_INFINITE);
|
321
|
330
|
watchdog = 0;
|
322
|
331
|
rt_mutex_release(&mutex_watchdog);
|
323
|
|
- //Close Camrera + etatCamera
|
|
332
|
+ rt_mutex_acquire(&mutex_etatCamera, TM_INFINITE);
|
|
333
|
+ etatCamera.arena = Arena();
|
|
334
|
+ etatCamera.askArena = 0;
|
|
335
|
+ etatCamera.cameraStarted = 0;
|
|
336
|
+ etatCamera.getImg = 0;
|
|
337
|
+ etatCamera.getPos = 0;
|
|
338
|
+ rt_mutex_release(&mutex_etatCamera);
|
|
339
|
+ camera.Close();
|
324
|
340
|
/* if (err = rt_task_delete(&th_server)) {
|
325
|
341
|
cerr << "Error task delete: " << strerror(-err) << endl << flush;
|
326
|
342
|
//exit(EXIT_FAILURE);
|
|
@@ -357,6 +373,48 @@ void Tasks::ReceiveFromMonTask(void *arg) {
|
357
|
373
|
rt_sem_v(&sem_startRobot);
|
358
|
374
|
cout << "On a start le robot avec Watchdog" << endl << flush;
|
359
|
375
|
}
|
|
376
|
+ else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)){
|
|
377
|
+ rt_sem_v(&sem_startCAM);
|
|
378
|
+ }
|
|
379
|
+ else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)){
|
|
380
|
+ camera.Close();
|
|
381
|
+ rt_mutex_acquire(&mutex_etatCamera, TM_INFINITE);
|
|
382
|
+ etatCamera.cameraStarted = 0;
|
|
383
|
+ etatCamera.arena = Arena();
|
|
384
|
+ etatCamera.getImg = 0;
|
|
385
|
+ etatCamera.getPos = 0;
|
|
386
|
+ etatCamera.askArena = 0;
|
|
387
|
+ rt_mutex_release(&mutex_etatCamera);
|
|
388
|
+ }
|
|
389
|
+ else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)){
|
|
390
|
+ rt_mutex_acquire(&mutex_etatCamera, TM_INFINITE);
|
|
391
|
+ etatCamera.askArena = 1;
|
|
392
|
+ etatCamera.getImg = 0;
|
|
393
|
+ rt_mutex_release(&mutex_etatCamera);
|
|
394
|
+ }
|
|
395
|
+ else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)){
|
|
396
|
+ rt_mutex_acquire(&mutex_etatCamera, TM_INFINITE);
|
|
397
|
+ etatCamera.askArena = 0;
|
|
398
|
+ etatCamera.getImg = 1;
|
|
399
|
+ rt_mutex_release(&mutex_etatCamera);
|
|
400
|
+ }
|
|
401
|
+ else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)){
|
|
402
|
+ rt_mutex_acquire(&mutex_etatCamera, TM_INFINITE);
|
|
403
|
+ etatCamera.arena = Arena();
|
|
404
|
+ etatCamera.askArena = 0;
|
|
405
|
+ etatCamera.getImg = 1;
|
|
406
|
+ rt_mutex_release(&mutex_etatCamera);
|
|
407
|
+ }
|
|
408
|
+ else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)){
|
|
409
|
+ rt_mutex_acquire(&mutex_etatCamera, TM_INFINITE);
|
|
410
|
+ etatCamera.getPos = 1;
|
|
411
|
+ rt_mutex_release(&mutex_etatCamera);
|
|
412
|
+ }
|
|
413
|
+ else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)){
|
|
414
|
+ rt_mutex_acquire(&mutex_etatCamera, TM_INFINITE);
|
|
415
|
+ etatCamera.getPos = 0;
|
|
416
|
+ rt_mutex_release(&mutex_etatCamera);
|
|
417
|
+ }
|
360
|
418
|
delete(msgRcv); // mus be deleted manually, no consumer
|
361
|
419
|
}
|
362
|
420
|
}
|
|
@@ -633,4 +691,94 @@ void Tasks::ReloadWatchdogTask(void *arg) {
|
633
|
691
|
}
|
634
|
692
|
}
|
635
|
693
|
|
|
694
|
+}
|
|
695
|
+
|
|
696
|
+/**
|
|
697
|
+ * @brief Starts the Camera and the aquisition of images
|
|
698
|
+ */
|
|
699
|
+void Tasks::StartCamera(void * arg){
|
|
700
|
+ bool resp;
|
|
701
|
+
|
|
702
|
+ // Synchronization barrier (waiting that all tasks are starting)
|
|
703
|
+ rt_sem_p(&sem_barrier, TM_INFINITE);
|
|
704
|
+
|
|
705
|
+ while(1){
|
|
706
|
+ //We wait for the signal startCam
|
|
707
|
+ rt_sem_p(&sem_startCAM, TM_INFINITE);
|
|
708
|
+ cout << "thread StartCam started" << endl << flush;
|
|
709
|
+ Message * msg;
|
|
710
|
+ resp = camera.Open();
|
|
711
|
+ if (resp){
|
|
712
|
+ cout << "thread StartCam ok" << endl << flush;
|
|
713
|
+ msg = new Message(MESSAGE_ANSWER_ACK);
|
|
714
|
+ rt_mutex_acquire(&mutex_etatCamera, TM_INFINITE);
|
|
715
|
+ etatCamera.cameraStarted = 1;
|
|
716
|
+ etatCamera.getImg = 1;
|
|
717
|
+ rt_mutex_release(&mutex_etatCamera);
|
|
718
|
+ }
|
|
719
|
+ else{
|
|
720
|
+ msg = new Message(MESSAGE_ANSWER_NACK);
|
|
721
|
+ }
|
|
722
|
+ WriteInQueue(&q_messageToMon,msg);
|
|
723
|
+ }
|
|
724
|
+}
|
|
725
|
+
|
|
726
|
+/**
|
|
727
|
+ * @brief The nominal task for the Camera
|
|
728
|
+ */
|
|
729
|
+void Tasks::CameraTask(void * arg){
|
|
730
|
+ struct etatCamera_t ec;
|
|
731
|
+ // Synchronization barrier (waiting that all tasks are starting)
|
|
732
|
+ rt_sem_p(&sem_barrier, TM_INFINITE);
|
|
733
|
+
|
|
734
|
+ cout << "thread Camera started" << endl << flush;
|
|
735
|
+ /**************************************************************************************/
|
|
736
|
+ /* The task starts here */
|
|
737
|
+ /**************************************************************************************/
|
|
738
|
+ rt_task_set_periodic(NULL, TM_NOW, 100000000);
|
|
739
|
+
|
|
740
|
+ while(1){
|
|
741
|
+ rt_task_wait_period(NULL);
|
|
742
|
+ rt_mutex_acquire(&mutex_etatCamera, TM_INFINITE);
|
|
743
|
+ ec = etatCamera;
|
|
744
|
+ rt_mutex_release(&mutex_etatCamera);
|
|
745
|
+
|
|
746
|
+ if (ec.cameraStarted == 1){
|
|
747
|
+ if (ec.getImg == 1){
|
|
748
|
+ Img img = camera.Grab();
|
|
749
|
+ if (ec.getPos == 1){
|
|
750
|
+ std::list<Position> pos;
|
|
751
|
+ pos = img.SearchRobot(ec.arena);
|
|
752
|
+ if (pos.empty()){
|
|
753
|
+ //Atention j'ai modifié img.h, classe Position attribut center dans le constructeur
|
|
754
|
+ pos.push_front(Position());
|
|
755
|
+ }
|
|
756
|
+ else{
|
|
757
|
+ img.DrawAllRobots(pos);
|
|
758
|
+ }
|
|
759
|
+ MessagePosition* msgPos = new MessagePosition(MESSAGE_CAM_POSITION, pos.front());
|
|
760
|
+ WriteInQueue(&q_messageToMon,msgPos);
|
|
761
|
+ }
|
|
762
|
+ MessageImg * msgImg;
|
|
763
|
+ msgImg = new MessageImg(MESSAGE_CAM_IMAGE, &img);
|
|
764
|
+ //WriteInQueue(&q_messageToMon,msgImg);
|
|
765
|
+ monitor.Write(msgImg);
|
|
766
|
+ }
|
|
767
|
+ else if (ec.askArena == 1){
|
|
768
|
+ Img img = camera.Grab();
|
|
769
|
+ ec.arena = img.SearchArena();
|
|
770
|
+ if (!ec.arena.IsEmpty()){
|
|
771
|
+ rt_mutex_acquire(&mutex_etatCamera, TM_INFINITE);
|
|
772
|
+ etatCamera.arena = ec.arena;
|
|
773
|
+ etatCamera.askArena = 0;
|
|
774
|
+ rt_mutex_release(&mutex_etatCamera);
|
|
775
|
+ img.DrawArena(ec.arena);
|
|
776
|
+ MessageImg * msgImg;
|
|
777
|
+ msgImg = new MessageImg(MESSAGE_CAM_IMAGE, &img);
|
|
778
|
+ monitor.Write(msgImg);
|
|
779
|
+ //WriteInQueue(&q_messageToMon,msgImg);
|
|
780
|
+ }
|
|
781
|
+ }
|
|
782
|
+ }
|
|
783
|
+ }
|
636
|
784
|
}
|