Merge remote-tracking branch 'origin/Camera' into evoxx-dumber-v3
# 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:
commit
312139f59e
6 changed files with 198 additions and 126 deletions
8
.idea/.gitignore
vendored
Normal file
8
.idea/.gitignore
vendored
Normal 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
20
.idea/misc.xml
Normal 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
6
.idea/vcs.xml
Normal file
|
@ -0,0 +1,6 @@
|
||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
<project version="4">
|
||||||
|
<component name="VcsDirectoryMappings">
|
||||||
|
<mapping directory="" vcs="Git" />
|
||||||
|
</component>
|
||||||
|
</project>
|
|
@ -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
|
||||||
|
|
|
@ -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);
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
|
|
||||||
|
|
Loading…
Reference in a new issue