alejeune/Temps-Reel_Lejeune_Robert_Lacroix
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

Merge remote-tracking branch 'origin/Camera' into evoxx-dumber-v3

Browse source 
# Conflicts:
#	software/raspberry/superviseur-robot/nbproject/private/configurations.xml
#	software/raspberry/superviseur-robot/nbproject/private/private.xml
#	software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.3-xenomai-22
#	software/raspberry/superviseur-robot/nbproject/private/timestamps-10.105.1.8-xenomai-22
#	software/raspberry/superviseur-robot/tasks.cpp
#	software/raspberry/superviseur-robot/tasks.h
This commit is contained in:
Raphaël LACROIX 2023-04-11 13:10:29 +02:00
commit 312139f59e
6 changed files with 198 additions and 126 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
.idea/.gitignore vendored Normal file
View file

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

20
.idea/misc.xml Normal file
View file

@ -0,0 +1,20 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ExternalStorageConfigurationManager" enabled="true" />
<component name="MakefileSettings">
<option name="linkedExternalProjectsSettings">
<MakefileProjectSettings>
<option name="externalProjectPath" value="$PROJECT_DIR$/software/raspberry/superviseur-robot" />
<option name="modules">
<set>
<option value="$PROJECT_DIR$/software/raspberry/superviseur-robot" />
</set>
</option>
<option name="version" value="2" />
</MakefileProjectSettings>
</option>
</component>
<component name="MakefileWorkspace" PROJECT_DIR="$PROJECT_DIR$/software/raspberry/superviseur-robot">
<contentRoot DIR="$PROJECT_DIR$" />
</component>
</project>

6
.idea/vcs.xml Normal file
View file

@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="VcsDirectoryMappings">
<mapping directory="" vcs="Git" />
</component>
</project>

2
software/raspberry/superviseur-robot/lib/img.h
View file

@ -39,7 +39,7 @@ using namespace std;
/** /**
* Redefinition of cv::Mat type * Redefinition of cv::Mat type
*/ */
typedef cv::Mat ImageMat; typedef cv::Mat ImageMat;
/** /**
* Declaration of Jpg type * Declaration of Jpg type

227
software/raspberry/superviseur-robot/tasks.cpp
View file

@ -22,12 +22,12 @@
#define PRIORITY_TSERVER 30 #define PRIORITY_TSERVER 30
#define PRIORITY_TOPENCOMROBOT 20 #define PRIORITY_TOPENCOMROBOT 20
#define PRIORITY_TMOVE 20 #define PRIORITY_TMOVE 20
#define PRIORITY_TSENDTOMON 22 #define PRIORITY_TSENDTOMON 23
#define PRIORITY_TRECEIVEFROMMON 25 #define PRIORITY_TRECEIVEFROMMON 25
#define PRIORITY_TSTARTROBOT 20 #define PRIORITY_TSTARTROBOT 20
#define PRIORITY_TCAMERA 21 #define PRIORITY_TCAMERA 21
#define PRIORITY_TBATTERY 25 #define PRIORITY_TBATTERY 19
#define PRIORITY_TSTARTCAMERA 25 #define PRIORITY_TSTARTCAMERA 22
#define PRIORITY_TROBOTRELOADMESSAGE 25 #define PRIORITY_TROBOTRELOADMESSAGE 25
/* /*
@ -46,50 +46,12 @@
* *
* 6- When you want to write something in terminal, use cout and terminate with endl and flush * 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, * @brief Initialisation des structures de l'application (tâches, mutex,
* semaphore, etc.) * 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){ Message* Tasks::parseMessage(Message* msg){
@ -190,6 +152,14 @@ void Tasks::Init() {
cerr << "Error mutex create: " << strerror(-err) << endl << flush; cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); 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; cout << "Mutexes created successfully" << endl << flush;
/**************************************************************************************/ /**************************************************************************************/
@ -264,10 +234,6 @@ void Tasks::Init() {
cerr << "Error task create: " << strerror(-err) << endl << flush; cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); 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)) { if (err = rt_task_create(&th_camera, "th_camera", 0, PRIORITY_TCAMERA, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush; cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
@ -286,12 +252,14 @@ void Tasks::Init() {
/**************************************************************************************/ /**************************************************************************************/
/* Message queues creation */ /* 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; cerr << "Error msg queue create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
cout << "Queues created successfully" << endl << flush; 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; cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); 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)) { if (err = rt_task_start(&th_robotReloadMessage, (void(*)(void*)) & Tasks::RobotReloadMessage, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush; cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
@ -461,18 +424,33 @@ void Tasks::ReceiveFromMonTask(void *arg) {
rt_mutex_release(&mutex_move); rt_mutex_release(&mutex_move);
} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)){ } else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)){
rt_sem_v(&sem_startCamera); 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)){ }else if(msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)){
cout << "Enabling search arena" << endl << flush;
rt_mutex_acquire(&mutex_search_arena, TM_INFINITE); rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
searchArena = 1; searchArena = 1;
rt_mutex_release(&mutex_search_arena); 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)){ }else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)){
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
robotPos = 1; robotPos = 1;
rt_mutex_release(&mutex_robot_pos); rt_mutex_release(&mutex_robot_pos);
cout << "Robot Pos true " << endl << flush;
}else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)){ }else if(msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)){
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE); rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
robotPos = 0; robotPos = 0;
rt_mutex_release(&mutex_robot_pos); rt_mutex_release(&mutex_robot_pos);
cout << "Robot Pos false" << endl << flush;
}else if(msgRcv->CompareID(MESSAGE_MONITOR_LOST)){ }else if(msgRcv->CompareID(MESSAGE_MONITOR_LOST)){
cout << "MONITOR LOST" << endl << flush; cout << "MONITOR LOST" << endl << flush;
@ -548,7 +526,7 @@ void Tasks::StartRobotTask(void *arg) {
rt_sem_p(&sem_startRobot, TM_INFINITE); rt_sem_p(&sem_startRobot, TM_INFINITE);
cout << "Start robot without watchdog ("; cout << "Start robot without watchdog (";
rt_mutex_acquire(&mutex_robot, TM_INFINITE); 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); rt_mutex_release(&mutex_robot);
cout << msgSend->GetID(); cout << msgSend->GetID();
cout << ")" << endl; cout << ")" << endl;
@ -659,32 +637,43 @@ void Tasks::StartCameraTask(void *arg) {
Message * msgSend; Message * msgSend;
Message * msg; Message * msg;
bool status;
rt_sem_p(&sem_startCamera, TM_INFINITE); rt_sem_p(&sem_startCamera, TM_INFINITE);
cout << "Start Camera "; cout << "Start Camera ";
rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
rt_mutex_acquire(&mutex_camera, 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){ if(!status){
msgSend = new Message(MESSAGE_ANSWER_NACK); msgSend = new Message(MESSAGE_ANSWER_NACK);
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
cout << "NACK !" << endl << flush ;
cout << msgSend->ToString() << endl << flush; cout << msgSend->ToString() << endl << flush;
cout << "NACK1 ! " << endl << flush ;
rt_mutex_release(&mutex_camera); rt_mutex_release(&mutex_camera);
cout << "NACK2 ! " << endl << flush ; rt_mutex_release(&mutex_cameraStarted);
continue; continue;
} }
msg = new Message(MESSAGE_ANSWER_ACK); msg = new Message(MESSAGE_ANSWER_ACK);
cout << "ACK3 ! " << endl << flush ;
cout << msg->ToString() << endl << flush; cout << msg->ToString() << endl << flush;
cout << "ACK4 ! " << endl << flush ; cameraStarted = !cameraStarted;
cout << cameraStarted << endl << flush;
rt_mutex_release(&mutex_camera); 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 <<flush;
rt_mutex_release(&mutex_cameraStarted); rt_mutex_release(&mutex_cameraStarted);
cout << "Cam success! " << endl <<flush;
cout << "Fin de start cam" << endl << flush; cout << "Fin de start cam" << endl << flush;
} }
} }
@ -749,7 +738,7 @@ void Tasks::BatteryTask(void *arg) {
rt_mutex_release(&mutex_robotStarted); rt_mutex_release(&mutex_robotStarted);
if (rs == 1) { if (rs == 1) {
rt_mutex_acquire(&mutex_robot, TM_INFINITE); rt_mutex_acquire(&mutex_robot, TM_INFINITE);
Message* batteryReply = parseMessage(robot.Write(robot.GetBattery())); // TODO : convert Message MessageID Message* batteryReply = parseMessage(robot.Write(robot.GetBattery()));
string batteryLevel = batteryReply->ToString(); string batteryLevel = batteryReply->ToString();
rt_mutex_release(&mutex_robot); rt_mutex_release(&mutex_robot);
WriteInQueue(&q_messageToMon, batteryReply); WriteInQueue(&q_messageToMon, batteryReply);
@ -760,13 +749,17 @@ void Tasks::BatteryTask(void *arg) {
} }
} }
/** /**
* @brief task in charge of managing the camera. * @brief task in charge of managing the camera.
*/ */
void Tasks::CameraTask(void *arg){ void Tasks::CameraTask(void *arg){
int cam; int cam;
int grab;
Img* image;
Arena arenaSaved,arena ;
std::list<Position> robotPosition;
MessageImg* imgToSend;
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting) // Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE); rt_sem_p(&sem_barrier, TM_INFINITE);
@ -776,44 +769,82 @@ void Tasks::CameraTask(void *arg){
rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE); rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
cam = cameraStarted; cam = cameraStarted;
rt_mutex_release(&mutex_cameraStarted); rt_mutex_release(&mutex_cameraStarted);
if(!cam){
if(!cam){
continue; continue;
} }
rt_mutex_acquire(&mutex_camera, TM_INFINITE); if(getImage){
// Lancer le traitement périodique cout << "Traitement de l'image" << endl << flush ;
Img img = camera.Grab(); rt_mutex_acquire(&mutex_camera, TM_INFINITE);
Arena arena; // Lancer le traitement périodique
rt_mutex_release(&mutex_camera);
rt_mutex_acquire(&mutex_search_arena, TM_INFINITE); 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){ if(searchArena){
// chopper l'arene rt_sem_p(&sem_answerSync, TM_INFINITE);
cout << "Search arena pls ! " << endl << flush; if(arenaConfirm){
arena = img.SearchArena(); // Arena OK
// searchArena = 0; // toggle? cout << "Arena Saved" << endl << flush;
} arenaSaved = arena;
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
}else{ }else{
cout << "Draw Arena" << endl << flush; // Arena not OK
img.DrawArena(arena); cout << "Arena Dumped" << endl << flush;
// Rien ?
// envoi de l'arene pour vérif }
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; rt_mutex_release(&mutex_answer_arena);
WriteInQueue(&q_messageToMon, new MessageImg(MESSAGE_CAM_IMAGE,&img)); rt_mutex_release(&mutex_search_arena);
} }
} }

13
software/raspberry/superviseur-robot/tasks.h
View file

@ -64,12 +64,14 @@ private:
/**********************************************************************/ /**********************************************************************/
ComMonitor monitor; ComMonitor monitor;
ComRobot robot; ComRobot robot;
Camera camera; Camera* camera;
int robotStarted = 0; int robotStarted = 0;
int compteurRobotLoss = 0; int compteurRobotLoss = 0;
int cameraStarted = 0;
int searchArena = 0;
int robotPos = 0; int robotPos = 0;
int cameraStarted = 0;
int arenaConfirm = 0;
int searchArena = 0;
int getImage = 0;
int move = MESSAGE_ROBOT_STOP; int move = MESSAGE_ROBOT_STOP;
/**********************************************************************/ /**********************************************************************/
@ -99,7 +101,11 @@ private:
RT_MUTEX mutex_compteurRobotLoss; RT_MUTEX mutex_compteurRobotLoss;
RT_MUTEX mutex_camera; RT_MUTEX mutex_camera;
RT_MUTEX mutex_search_arena; RT_MUTEX mutex_search_arena;
RT_MUTEX mutex_answer_arena;
RT_MUTEX mutex_robot_pos; RT_MUTEX mutex_robot_pos;
RT_MUTEX mutex_battery;
RT_MUTEX mutex_image;
/**********************************************************************/ /**********************************************************************/
/* Semaphores */ /* Semaphores */
@ -110,6 +116,7 @@ private:
RT_SEM sem_startRobot; RT_SEM sem_startRobot;
RT_SEM sem_startRobotWithWatchdog; RT_SEM sem_startRobotWithWatchdog;
RT_SEM sem_startCamera; RT_SEM sem_startCamera;
RT_SEM sem_answerSync;
RT_SEM sem_connexionEstablished; RT_SEM sem_connexionEstablished;
RT_SEM sem_reloadMessages; RT_SEM sem_reloadMessages;