diff --git a/.idea/.gitignore b/.idea/.gitignore
new file mode 100644
index 0000000..13566b8
--- /dev/null
+++ b/.idea/.gitignore
@@ -0,0 +1,8 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Editor-based HTTP Client requests
+/httpRequests/
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml
diff --git a/.idea/misc.xml b/.idea/misc.xml
new file mode 100644
index 0000000..6a19798
--- /dev/null
+++ b/.idea/misc.xml
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/vcs.xml b/.idea/vcs.xml
new file mode 100644
index 0000000..35eb1dd
--- /dev/null
+++ b/.idea/vcs.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/software/raspberry/superviseur-robot/lib/img.h b/software/raspberry/superviseur-robot/lib/img.h
index 11822e0..191d5c6 100644
--- a/software/raspberry/superviseur-robot/lib/img.h
+++ b/software/raspberry/superviseur-robot/lib/img.h
@@ -39,7 +39,7 @@ using namespace std;
/**
* Redefinition of cv::Mat type
*/
-typedef cv::Mat ImageMat;
+ typedef cv::Mat ImageMat;
/**
* Declaration of Jpg type
diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp
index 5fae393..cb48d5b 100644
--- a/software/raspberry/superviseur-robot/tasks.cpp
+++ b/software/raspberry/superviseur-robot/tasks.cpp
@@ -22,12 +22,12 @@
#define PRIORITY_TSERVER 30
#define PRIORITY_TOPENCOMROBOT 20
#define PRIORITY_TMOVE 20
-#define PRIORITY_TSENDTOMON 22
+#define PRIORITY_TSENDTOMON 23
#define PRIORITY_TRECEIVEFROMMON 25
#define PRIORITY_TSTARTROBOT 20
#define PRIORITY_TCAMERA 21
-#define PRIORITY_TBATTERY 25
-#define PRIORITY_TSTARTCAMERA 25
+#define PRIORITY_TBATTERY 19
+#define PRIORITY_TSTARTCAMERA 22
#define PRIORITY_TROBOTRELOADMESSAGE 25
/*
@@ -46,59 +46,21 @@
*
* 6- When you want to write something in terminal, use cout and terminate with endl and flush
*
- * 7- Good luck !
+ * 7- Good luck ! Thanks
*/
/**
* @brief Initialisation des structures de l'application (tâches, mutex,
* semaphore, etc.)
-void Tasks::RobotLossCounter(void* arg){
- int state;
-
- // wait for connexion establishement
- 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_connexionEstablished, TM_INFINITE);
- cout << "Starting to monitor packet losses " << endl << flush;
- rt_task_set_periodic(NULL, TM_NOW, 100*1000*1000);
- while(1){
- rt_task_wait_period(NULL);
-
- rt_mutex_acquire(&mutex_compteurRobotLoss, TM_INFINITE);
- if(compteurRobotLoss >= 3){
- // send info to monitor
- WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_COM_ERROR)); // TODO : c'est probablement faux mais on a suivi la doc *sig*
-
- // close robot
- rt_mutex_acquire(&mutex_robot, TM_INFINITE);
- state = robot.Close();
- rt_mutex_release(&mutex_robot);
- rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
- robotStarted = 0;
- rt_mutex_release(&mutex_robotStarted);
-
-
- // send info to monitor
- if (state < 0) {
- WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
- } else {
- WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
- }
- }
- rt_mutex_release(&mutex_compteurRobotLoss);
- }
-
-}
*/
Message* Tasks::parseMessage(Message* msg){
int state;
-
+
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
int tempRobotStarted = robotStarted ;
rt_mutex_release(&mutex_robotStarted);
-
+
if (tempRobotStarted != 0) {
if (msg->CompareID(MESSAGE_ANSWER_NACK) ||msg->CompareID(MESSAGE_ANSWER_ROBOT_TIMEOUT) ||msg->CompareID(MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND) ||msg->CompareID(MESSAGE_ANSWER_ROBOT_ERROR) ||msg->CompareID(MESSAGE_ANSWER_COM_ERROR)){
@@ -190,6 +152,14 @@ void Tasks::Init() {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
+ if (err = rt_mutex_create(&mutex_battery, NULL)) {
+ cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+ exit(EXIT_FAILURE);
+ }
+ if (err = rt_mutex_create(&mutex_image, NULL)) {
+ cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+ exit(EXIT_FAILURE);
+ }
cout << "Mutexes created successfully" << endl << flush;
/**************************************************************************************/
@@ -264,10 +234,6 @@ void Tasks::Init() {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
- /*if (err = rt_task_create(&th_lossCounter, "th_lossCounter", 0, PRIORITY_TLOSSCOUNTER, 0)) {
- cerr << "Error task create: " << strerror(-err) << endl << flush;
- exit(EXIT_FAILURE);
- }*/
if (err = rt_task_create(&th_camera, "th_camera", 0, PRIORITY_TCAMERA, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
@@ -280,18 +246,20 @@ void Tasks::Init() {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
-
+
cout << "Tasks created successfully" << endl << flush;
/**************************************************************************************/
/* Message queues creation */
/**************************************************************************************/
- if ((err = rt_queue_create(&q_messageToMon, "q_messageToMon", sizeof (Message*)*50, Q_UNLIMITED, Q_FIFO)) < 0) {
+ if ((err = rt_queue_create(&q_messageToMon, "q_messageToMon", sizeof (Message*)*100, Q_UNLIMITED, Q_FIFO)) < 0) {
cerr << "Error msg queue create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Queues created successfully" << endl << flush;
+ camera = new Camera(sm,5);
+ cout << "New camera ! " << endl << flush;
}
/**
@@ -337,11 +305,6 @@ void Tasks::Run() {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
-
- /*if (err = rt_task_start(&th_lossCounter, (void(*)(void*)) & Tasks::RobotLossCounter, this)) {
- cerr << "Error task start: " << strerror(-err) << endl << flush;
- exit(EXIT_FAILURE);
- }*/
if (err = rt_task_start(&th_robotReloadMessage, (void(*)(void*)) & Tasks::RobotReloadMessage, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
@@ -461,35 +424,50 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_mutex_release(&mutex_move);
} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)){
rt_sem_v(&sem_startCamera);
+ } else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)){
+ rt_sem_v(&sem_startCamera);
}else if(msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)){
+ cout << "Enabling search arena" << endl << flush;
rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
searchArena = 1;
rt_mutex_release(&mutex_search_arena);
+ rt_mutex_acquire(&mutex_image, TM_INFINITE);
+ getImage = 0;
+ rt_mutex_release(&mutex_image);
+ cout << "Search arena true " << endl << flush;
+ } else if(msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM) || msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)){
+ rt_mutex_acquire(&mutex_answer_arena,TM_INFINITE);
+ arenaConfirm = msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)? 1:0; // A vérifier que le ternaire est utile
+ cout << "Statut de l'arene" << arenaConfirm << endl << flush;
+ rt_mutex_release(&mutex_answer_arena);
+ rt_sem_v(&sem_answerSync);
}else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)){
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
robotPos = 1;
rt_mutex_release(&mutex_robot_pos);
+ cout << "Robot Pos true " << endl << flush;
}else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)){
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
robotPos = 0;
rt_mutex_release(&mutex_robot_pos);
+ cout << "Robot Pos false" << endl << flush;
}else if(msgRcv->CompareID(MESSAGE_MONITOR_LOST)){
cout << "MONITOR LOST" << endl << flush;
-
+
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
robot.Close();
rt_mutex_release(&mutex_robot);
-
-
+
+
rt_mutex_acquire(&mutex_camera, TM_INFINITE);
camera.Close();
rt_mutex_release(&mutex_camera);
-
-
+
+
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
robotStarted = 0;
rt_mutex_release(&mutex_robotStarted);
-
+
}
delete(msgRcv); // must be deleted manually, no consumer
}
@@ -525,7 +503,7 @@ void Tasks::OpenComRobot(void *arg) {
rt_sem_v(&sem_connexionEstablished);
msgSend = new Message(MESSAGE_ANSWER_ACK);
}
-
+
cout << "Open serial com (";
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
}
@@ -538,7 +516,7 @@ void Tasks::StartRobotTask(void *arg) {
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
-
+
/**************************************************************************************/
/* The task startRobot starts here */
/**************************************************************************************/
@@ -548,7 +526,7 @@ void Tasks::StartRobotTask(void *arg) {
rt_sem_p(&sem_startRobot, TM_INFINITE);
cout << "Start robot without watchdog (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
- msgSend = parseMessage(robot.Write(robot.StartWithoutWD())); // TODO : convert Message MessageID
+ msgSend = parseMessage(robot.Write(robot.StartWithoutWD()));
rt_mutex_release(&mutex_robot);
cout << msgSend->GetID();
cout << ")" << endl;
@@ -610,20 +588,20 @@ void Tasks::RobotReloadMessage(void *arg) {
int status;
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_reloadMessages, TM_INFINITE);
-
-
-
+
+
+
cout << "Starting to monitor packet losses " << endl << flush;
rt_task_set_periodic(NULL, TM_NOW, 1000*1000*1000);
while(1) {
rt_task_wait_period(NULL);
-
+
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
int tempRobotStarted = robotStarted ;
@@ -631,7 +609,7 @@ void Tasks::RobotReloadMessage(void *arg) {
if (tempRobotStarted != 0) {
rt_mutex_acquire(&mutex_robot, TM_INFINITE);
- status = parseMessage(robot.Write(robot.ReloadWD()))->GetID();
+ status = parseMessage(robot.Write(robot.ReloadWD()))->GetID();
rt_mutex_release(&mutex_robot);
// send info to monitor
if (status < 0) {
@@ -651,7 +629,7 @@ void Tasks::StartCameraTask(void *arg) {
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
-
+
/**************************************************************************************/
/* The task startRobot starts here */
/**************************************************************************************/
@@ -659,32 +637,43 @@ void Tasks::StartCameraTask(void *arg) {
Message * msgSend;
Message * msg;
+ bool status;
rt_sem_p(&sem_startCamera, TM_INFINITE);
cout << "Start Camera ";
+ rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
rt_mutex_acquire(&mutex_camera, TM_INFINITE);
- bool status = camera.Open();
+ if(!cameraStarted){
+ status = camera->Open();
+ cout << "Camera Opened" << endl << flush; // Toggle a modifier ?
+ rt_mutex_acquire(&mutex_image, TM_INFINITE);
+ getImage = 1;
+ rt_mutex_release(&mutex_image);
+ }else{
+ camera->Close();
+ status = true;
+ rt_mutex_acquire(&mutex_image, TM_INFINITE);
+ getImage = 0;
+ rt_mutex_release(&mutex_image);
+ cout << "Camera Closed" << endl << flush;
+ }
+
if(!status){
msgSend = new Message(MESSAGE_ANSWER_NACK);
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
- cout << "NACK !" << endl << flush ;
cout << msgSend->ToString() << endl << flush;
- cout << "NACK1 ! " << endl << flush ;
rt_mutex_release(&mutex_camera);
- cout << "NACK2 ! " << endl << flush ;
+ rt_mutex_release(&mutex_cameraStarted);
continue;
}
msg = new Message(MESSAGE_ANSWER_ACK);
- cout << "ACK3 ! " << endl << flush ;
cout << msg->ToString() << endl << flush;
- cout << "ACK4 ! " << endl << flush ;
+ cameraStarted = !cameraStarted;
+ cout << cameraStarted << endl << flush;
rt_mutex_release(&mutex_camera);
- cout << "ACK ! " << endl << flush ;
- rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
- cout << "ff" << endl << flush ;
- cameraStarted = 1;
- cout << "Cam success! " << endl <ToString();
rt_mutex_release(&mutex_robot);
WriteInQueue(&q_messageToMon, batteryReply);
}
-
+
}
}
-
/**
* @brief task in charge of managing the camera.
*/
void Tasks::CameraTask(void *arg){
-
int cam;
+ int grab;
+ Img* image;
+ Arena arenaSaved,arena ;
+ std::list robotPosition;
+ MessageImg* imgToSend;
+
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
@@ -776,44 +769,82 @@ void Tasks::CameraTask(void *arg){
rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
cam = cameraStarted;
rt_mutex_release(&mutex_cameraStarted);
- if(!cam){
+ if(!cam){
continue;
}
- rt_mutex_acquire(&mutex_camera, TM_INFINITE);
- // Lancer le traitement périodique
- Img img = camera.Grab();
- Arena arena;
- rt_mutex_release(&mutex_camera);
- rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
+ if(getImage){
+ cout << "Traitement de l'image" << endl << flush ;
+ rt_mutex_acquire(&mutex_camera, TM_INFINITE);
+ // Lancer le traitement périodique
+
+ Img img = camera->Grab();
+ image = img.Copy();
+ rt_mutex_release(&mutex_camera);
+ rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
+ arena = image->SearchArena(); // Mise a jour de l'arene tout le temps pour la position du robot mais draw si search arena = true
+ if(searchArena){
+ rt_mutex_acquire(&mutex_image, TM_INFINITE);
+ getImage = 0;
+ rt_mutex_release(&mutex_image);
+ // chopper l'arene
+ if(arena.IsEmpty()){
+ cout << "Arena empty" << endl << flush;
+ // envoi d'un no ack au moniteur
+
+ }else{
+ cout << "Draw Arena" << endl << flush;
+ image->DrawArena(arena);
+ // envoi de l'arene pour vérif
+ }
+ }else {
+ image->DrawArena(arenaSaved);
+ }
+
+ rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
+ if(robotPos){
+ // chopper le robot
+ cout << "Searching robot positions" << endl << flush ;
+ robotPosition = image->SearchRobot(arena);
+
+ //Traitement du robot
+ if(!robotPosition.empty()){ // Robot position != null
+ image->DrawAllRobots(robotPosition);
+ }
+ for (auto position : robotPosition)
+ {
+ WriteInQueue(&q_messageToMon, new MessagePosition(MESSAGE_CAM_POSITION,position)); // Envoi de toutes les positions
+ }
+ }
+ rt_mutex_release(&mutex_robot_pos);
+ // Envoi de l'image
+ cout << "Envoi de l'image" << endl << flush ;
+ imgToSend = new MessageImg(MESSAGE_CAM_IMAGE,image);
+ WriteInQueue(&q_messageToMon, imgToSend);
+
+ }
+
+
+ rt_mutex_acquire(&mutex_answer_arena, TM_INFINITE);
if(searchArena){
- // chopper l'arene
- cout << "Search arena pls ! " << endl << flush;
- arena = img.SearchArena();
- // searchArena = 0; // toggle?
- }
- rt_mutex_release(&mutex_search_arena);
-
- rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
- if(robotPos){
- // chopper le robot
- cout << "Pos du robot pls !" ;
-
- }
- rt_mutex_release(&mutex_robot_pos);
- if(arena.IsEmpty()){
- cout << "Arena empty" << endl << flush;
- // envoi d'un no ack au moniteur
+ rt_sem_p(&sem_answerSync, TM_INFINITE);
+ if(arenaConfirm){
+ // Arena OK
+ cout << "Arena Saved" << endl << flush;
+ arenaSaved = arena;
}else{
- cout << "Draw Arena" << endl << flush;
- img.DrawArena(arena);
-
- // envoi de l'arene pour vérif
+ // Arena not OK
+ cout << "Arena Dumped" << endl << flush;
+ // Rien ?
+ }
+ rt_mutex_acquire(&mutex_image, TM_INFINITE);
+ getImage = 1; // changer en booleen pour synchroniser plutot que ça
+ rt_mutex_release(&mutex_image);
+ searchArena = 0;
}
- cout << "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAARENE" << endl << flush;
- WriteInQueue(&q_messageToMon, new MessageImg(MESSAGE_CAM_IMAGE,&img));
-
+ rt_mutex_release(&mutex_answer_arena);
+ rt_mutex_release(&mutex_search_arena);
}
}
diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h
index d8b2812..046c14b 100644
--- a/software/raspberry/superviseur-robot/tasks.h
+++ b/software/raspberry/superviseur-robot/tasks.h
@@ -64,12 +64,14 @@ private:
/**********************************************************************/
ComMonitor monitor;
ComRobot robot;
- Camera camera;
+ Camera* camera;
int robotStarted = 0;
int compteurRobotLoss = 0;
- int cameraStarted = 0;
- int searchArena = 0;
int robotPos = 0;
+ int cameraStarted = 0;
+ int arenaConfirm = 0;
+ int searchArena = 0;
+ int getImage = 0;
int move = MESSAGE_ROBOT_STOP;
/**********************************************************************/
@@ -87,7 +89,7 @@ private:
RT_TASK th_camera;
RT_TASK th_startRobotWithWatchdog;
RT_TASK th_robotReloadMessage;
-
+
/**********************************************************************/
/* Mutex */
/**********************************************************************/
@@ -99,7 +101,11 @@ private:
RT_MUTEX mutex_compteurRobotLoss;
RT_MUTEX mutex_camera;
RT_MUTEX mutex_search_arena;
+ RT_MUTEX mutex_answer_arena;
RT_MUTEX mutex_robot_pos;
+ RT_MUTEX mutex_battery;
+ RT_MUTEX mutex_image;
+
/**********************************************************************/
/* Semaphores */
@@ -110,6 +116,7 @@ private:
RT_SEM sem_startRobot;
RT_SEM sem_startRobotWithWatchdog;
RT_SEM sem_startCamera;
+ RT_SEM sem_answerSync;
RT_SEM sem_connexionEstablished;
RT_SEM sem_reloadMessages;
@@ -151,7 +158,7 @@ private:
* @brief Thread starting the communication with the robot with a watchdog.
*/
void StartRobotWithWatchDogTask(void *arg);
-
+
/**
* @brief Thread starting the communication with the Camera.
*/
@@ -201,7 +208,7 @@ private:
* @return Message
*/
Message* parseMessage(Message* msg);
-
+
/**
* Reloads a message
*/