Browse Source

Camera fonctionelle

Tali Elies 8 months ago
parent
commit
3520385196

+ 1
- 1
software/raspberry/superviseur-robot/lib/img.h View File

@@ -64,7 +64,7 @@ public:
64 64
     Position() {
65 65
         robotId = -1;
66 66
         angle = 0.0;
67
-        center=cv::Point2f(0.0,0.0);
67
+        center=cv::Point2f(-1.0,-1.0); //Remettre 0.0 si jamais
68 68
         direction=cv::Point2f(0.0,0.0);
69 69
     }
70 70
     

+ 1
- 1
software/raspberry/superviseur-robot/nbproject/private/configurations.xml View File

@@ -41,7 +41,7 @@
41 41
     </conf>
42 42
     <conf name="Debug__RPI_" type="1">
43 43
       <toolsSet>
44
-        <developmentServer>pi@10.105.1.11:22</developmentServer>
44
+        <developmentServer>pi@10.105.1.04:22</developmentServer>
45 45
         <platform>2</platform>
46 46
       </toolsSet>
47 47
       <dbx_gdbdebugger version="1">

+ 0
- 0
software/raspberry/superviseur-robot/nbproject/private/downloads-10.105.1.04-pi-22 View File


+ 39
- 0
software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.04-pi-22 View File

@@ -0,0 +1,39 @@
1
+#Wed Mar 17 11:17:08 CET 2021
2
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/scriptRasp.sh=c1614326506000
3
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/lib/camera.cpp=c1614324985000
4
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/lib/img.cpp=c1614324985000
5
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp=c1614324985000
6
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/lib/base64/test.cpp=c1614324985000
7
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/nbproject/project.xml=c1614324985000
8
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp=c1614324985000
9
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/lib/base64/base64.h=c1614324985000
10
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/lib/base64/LICENSE=c1614324985000
11
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/nbproject/project.properties=c1614324985000
12
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/tasks.h=c1615975786000
13
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/lib/img.h=c1615972510000
14
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/lib/messages.h=c1614324985000
15
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/tasks.cpp=c1615976224000
16
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1614324985000
17
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/lib/.gitignore=c1614324985000
18
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1614324985000
19
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/gdbsudo.sh=c1614324985000
20
+VERSION=1.3
21
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/lib/commonitor.h=c1614324985000
22
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/README.md=c1614324985000
23
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/lib/base64/README.md=c1614324985000
24
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/.dep.inc=c1614324985000
25
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/lib/comrobot.h=c1614324985000
26
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/lib/base64/.gitignore=c1614324985000
27
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk=c1614324985000
28
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/nbproject/Package-Debug__RPI_.bash=c1615797916000
29
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/Makefile=c1614324985000
30
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk=c1615797916000
31
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk=c1615797916000
32
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/.gitignore=c1614324985000
33
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/lib/messages.cpp=c1614324985000
34
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__PC_.mk=c1614324985000
35
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/lib/base64/compile-and-run-test=c1614324985000
36
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/superviseur.doxygen=c1614324985000
37
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/lib/base64/base64.cpp=c1614324985000
38
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/lib/camera.h=c1614324985000
39
+/home/tali/Temps_Reel/dumber/software/raspberry/superviseur-robot/main.cpp=c1614324985000

+ 151
- 3
software/raspberry/superviseur-robot/tasks.cpp View File

@@ -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
 }

+ 12
- 1
software/raspberry/superviseur-robot/tasks.h View File

@@ -64,13 +64,14 @@ private:
64 64
     /**********************************************************************/
65 65
     ComMonitor monitor;
66 66
     ComRobot robot;
67
+    Camera camera = Camera(sm,10);
67 68
     int robotStarted = 0;
68 69
     int move = MESSAGE_ROBOT_STOP;
69 70
     struct etatCamera_t{
70 71
         int cameraStarted = 0;
71 72
         int getImg = 0;
72 73
         int askArena = 0;
73
-        Arena* arena = new Arena();
74
+        Arena arena = Arena();
74 75
         int getPos = 0;
75 76
     };
76 77
     struct etatCamera_t etatCamera;
@@ -168,6 +169,16 @@ private:
168 169
     * @brief Kills and restart the thread watchdog
169 170
     */
170 171
     void Kill_StartWD();
172
+    
173
+    /**
174
+    * @brief Starts the Camera and the aquisition of images
175
+    */
176
+    void StartCamera(void * arg);
177
+    
178
+    /**
179
+    * @brief The nominal task for the Camera
180
+    */
181
+    void CameraTask(void * arg);
171 182
    
172 183
     /**********************************************************************/
173 184
     /* Queue services                                                     */

Loading…
Cancel
Save