Camera presque finie

This commit is contained in:
alejeune 2023-04-03 15:21:01 +02:00
parent 96a3c8fe08
commit a9194dee2d
6 changed files with 160 additions and 41 deletions

View file

@ -41,7 +41,7 @@
</conf>
<conf name="Debug__RPI_" type="1">
<toolsSet>
<developmentServer>xenomai@10.105.1.5:22</developmentServer>
<developmentServer>xenomai@10.105.1.8:22</developmentServer>
<platform>2</platform>
</toolsSet>
<dbx_gdbdebugger version="1">

View file

@ -6,6 +6,13 @@
</data>
<editor-bookmarks xmlns="http://www.netbeans.org/ns/editor-bookmarks/2" lastBookmarkId="0"/>
<open-files xmlns="http://www.netbeans.org/ns/projectui-open-files/2">
<group/>
<group>
<file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.h</file>
<file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.h</file>
<file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.cpp</file>
<file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.cpp</file>
<file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/main.cpp</file>
<file>file:/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.h</file>
</group>
</open-files>
</project-private>

View file

@ -0,0 +1,39 @@
#Mon Apr 03 15:19:02 CEST 2023
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Package-Debug__RPI_.bash=c1679401432930
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.cpp=c1680248859421
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/commonitor.cpp=c1679401432864
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/gdbsudo.sh=c1679401432813
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/LICENSE=c1679401432832
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/comrobot.cpp=c1679401457314
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.cpp=c1680527409352
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/.dep.inc=c1679401432701
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-variables.mk=c1679401432922
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/img.cpp=c1679401457320
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.h=c1680518630237
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/README.md=c1679401432714
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__PC_.mk=c1679401457337
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/Makefile=c1679401432709
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/.gitignore=c1679401432820
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/wrapper.c=c1679401457381
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/tasks.h=c1680519967821
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/main.cpp=c1679404095224
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/project.properties=c1679401432983
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/camera.cpp=c1679401457304
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/test.cpp=c1679401432851
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/base64.h=c1679401432844
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-Debug__RPI_.mk=c1679401457346
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Package-Debug__PC_.bash=c1679401432926
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/project.xml=c1679401457377
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/.gitignore=c1679401432827
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/Makefile-impl.mk=c1679401432918
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/nbproject/private/Makefile-variables.mk=c1679401432944
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/superviseur.doxygen=c1679401432993
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/messages.h=c1679401432897
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/base64.cpp=c1679401432840
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/comrobot.h=c1679401432877
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/img.h=c1679401457331
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/.gitignore=c1679401432706
VERSION=1.3
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/commonitor.h=c1679401432868
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/README.md=c1679401432836
/home/alejeune/Documents/4ir/S2/dumber/software/raspberry/superviseur-robot/lib/base64/compile-and-run-test=c1679401432848

View file

@ -27,6 +27,8 @@
#define PRIORITY_TSTARTROBOT 20
#define PRIORITY_TCAMERA 21
#define PRIORITY_TBATTERY 25
#define PRIORITY_TSTARTCAMERA 25
/*
* Some remarks:
@ -74,6 +76,22 @@ void Tasks::Init() {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_cameraStarted, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_robot_pos, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_search_arena, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_mutex_create(&mutex_camera, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Mutexes created successfully" << endl << flush;
/**************************************************************************************/
@ -95,6 +113,10 @@ void Tasks::Init() {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_sem_create(&sem_startCamera, NULL, 0, S_FIFO)) {
cerr << "Error semaphore create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Semaphores created successfully" << endl << flush;
/**************************************************************************************/
@ -120,6 +142,7 @@ void Tasks::Init() {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
@ -128,6 +151,14 @@ void Tasks::Init() {
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);
}
if (err = rt_task_create(&th_startCamera, "th_startCamera", 0, PRIORITY_TSTARTCAMERA, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Tasks created successfully" << endl << flush;
/**************************************************************************************/
@ -176,7 +207,15 @@ void Tasks::Run() {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_camera, (void(*)(void*)) & Tasks::CameraTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_startCamera, (void(*)(void*)) & Tasks::StartCameraTask, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
cout << "Tasks launched" << endl << flush;
}
@ -240,7 +279,7 @@ void Tasks::SendToMonTask(void* arg) {
while (1) {
cout << "wait msg to send" << endl << flush;
msg = ReadInQueue(&q_messageToMon);
cout << "Send msg to mon: " << msg->ToString() << endl << flush;
//cout << "Send msg to mon: " << msg->ToString() << endl << flush;
rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
monitor.Write(msg); // The message is deleted with the Write
rt_mutex_release(&mutex_monitor);
@ -285,12 +324,31 @@ 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_ARENA_CONFIRM) || msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)){
rt_mutex_acquire(&mutex_answer_arena);
msg_arena = msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM? 1:0; // A vérifier que le ternaire est utile
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 << "Staut de l'arene" << arenaConfirm << endl << flush;
rt_mutex_release(&mutex_answer_arena);
rt_sem_v(&sem_answerSync);
}else if(msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)){
rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
searchArena = 1;
rt_mutex_release(&mutex_search_arena);
cout << "Search arena true " << endl << flush;
}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;
}
delete(msgRcv); // must be deleted manually, no consumer
}
}
@ -365,35 +423,47 @@ void Tasks::StartRobotTask(void *arg) {
* @brief Thread starting the communication with the robot.
*/
void Tasks::StartCameraTask(void *arg) {
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
// Synchronization barrier (waiting that all tasks are starting)
rt_sem_p(&sem_barrier, TM_INFINITE);
/**************************************************************************************/
/* The task Camera Task starts here */
/* The task startRobot starts here */
/**************************************************************************************/
while (1) {
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);
if(!camera.Open()){
msgSend = new Message(MESSAGE_ANSWER_NACK)
if(!cameraStarted){
status = camera.Open();
cout << "Camera Opened" << endl << flush;
}else{
camera.Close();
status = true;
cout << "Camera Closed" << endl << flush;
}
if(!status){
msgSend = new Message(MESSAGE_ANSWER_NACK);
WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
cout << msgSend->GetID << endl << flush;
cout << msgSend->ToString() << endl << flush;
rt_mutex_release(&mutex_camera);
rt_mutex_release(&mutex_cameraStarted);
continue;
}
msg = new Message(MESSAGE_ANSWER_ACK);
cout << msgSend->GetID << endl << flush;
msg = new Message(MESSAGE_ANSWER_ACK);
cout << msg->ToString() << endl << flush;
cameraStarted = !cameraStarted;
cout << cameraStarted << endl << flush;
rt_mutex_release(&mutex_camera);
rt_mutex_release(&mutex_cameraStarted);
if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
rt_mutex_acquire(&mutex_cameraStarted, TM_INFINITE);
cameraStarted = 1;
rt_mutex_release(&mutex_cameraStarted);
}
cout << "Cam success! " << endl <<flush;
cout << "Fin de start cam" << endl << flush;
}
}
@ -415,7 +485,7 @@ void Tasks::MoveTask(void *arg) {
while (1) {
rt_task_wait_period(NULL);
cout << "Periodic movement update";
//cout << "Periodic movement update";
rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
rs = robotStarted;
if (rs == 1) {
@ -430,7 +500,7 @@ void Tasks::MoveTask(void *arg) {
rt_mutex_release(&mutex_robot);
}
rt_mutex_release(&mutex_robotStarted);
cout << endl << flush;
//cout << endl << flush;
}
}
@ -470,7 +540,7 @@ void Tasks::BatteryTask(void *arg) {
/**
* @brief task in charge of managing the camera.
*/
void CameraTask(void *arg){
void Tasks::CameraTask(void *arg){
int cam;
std::list<Position> robotPosition;
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
@ -495,7 +565,6 @@ void CameraTask(void *arg){
rt_mutex_acquire(&mutex_search_arena, TM_INFINITE);
if(searchArena){
// chopper l'arene
cout << "Search arena pls ! " << endl << flush;
if(arena.IsEmpty()){
cout << "Arena empty" << endl << flush;
// envoi d'un no ack au moniteur
@ -505,47 +574,47 @@ void CameraTask(void *arg){
img.DrawArena(arena);
// envoi de l'arene pour vérif
cout << "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAARENE" << endl << flush;
}
}
rt_mutex_release(&mutex_search_arena);
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
rt_mutex_acquire(&mutex_robot_pos, TM_INFINITE);
if(robotPos){
// chopper le robot
cout << "Pos du robot pls !" ;
robotPosition = img.SearchRobot(arena);
//Traitement du robot
if(!robotPosition.empty()){ // Robot position != null
img.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);
//Traitement du robot
if(){ // Robot position != null
img.DrawAllRobots(robotPosition);
}
for (auto position : robotPosition)
{
WriteInQueue(&q_messageToMon, position); // Envoi de toutes les positions
}
// Envoi de l'image
WriteInQueue(&q_messageToMon, new MessageImg(MESSAGE_CAM_IMAGE,&img));
rt_sem_p(&sem_answerSync, TM_INFINITE);
rt_mutex_acquire(&mutex_anwer_arena, TM_INFINITE);
rt_mutex_acquire(&mutex_answer_arena, TM_INFINITE);
if(arenaConfirm){
// Arena OK
img.DrawArena(arena); // Comment save ou rejeter?
//searchArena = 0;
}else{
// Arena not OK
searchArena = 0;
// Rien ?
}
rt_mutex_release(&mutex_answer_arena);
rt_mutex_release(&mutex_search_arena);
// Je sais pas si cette partie sert a qqc
}
}
}
/**

View file

@ -66,8 +66,10 @@ private:
ComRobot robot;
Camera camera;
int robotStarted = 0;
int robotPos = 0;
int cameraStarted = 0;
int arenaConfirm = 0;
int searchArena = 0;
int move = MESSAGE_ROBOT_STOP;
/**********************************************************************/
@ -91,7 +93,9 @@ private:
RT_MUTEX mutex_cameraStarted;
RT_MUTEX mutex_move;
RT_MUTEX mutex_camera;
RT_MUTEX mutex_search_arena;
RT_MUTEX mutex_answer_arena;
RT_MUTEX mutex_robot_pos;
/**********************************************************************/
/* Semaphores */