INSA/dumber
1
0
Fork 0
mirror of https://github.com/yoboujon/dumber.git synced 2025-06-08 13:50:49 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Fixed issue with cam opening/closing. Changing how Camera behaves and Arena

Browse source
This commit is contained in:
Yohan Boujon 2024-05-11 14:35:51 +02:00
parent e018b5d0f4
commit 61b7855f3c
2 changed files with 93 additions and 79 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -90,7 +90,7 @@ 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_arenaList, NULL)) { if (err = rt_mutex_create(&mutex_arena, NULL)) {
cerr << "Error mutex create: " << strerror(-err) << endl << flush; cerr << "Error mutex create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
@ -153,7 +153,7 @@ 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_cameraOpen, "th_cameraOpen", 0, PRIORITY_TSETCAMERA, 0)) { if (err = rt_task_create(&th_cameraManage, "th_cameraManage", 0, PRIORITY_TSETCAMERA, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush; cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
@ -161,10 +161,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_cameraClose, "th_cameraClose", 0, PRIORITY_TSETCAMERA, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_create(&th_arenaChoice, "th_arenaChoice", 0, PRIOTITY_TARENA, 0)) { if (err = rt_task_create(&th_arenaChoice, "th_arenaChoice", 0, PRIOTITY_TARENA, 0)) {
cerr << "Error task create: " << strerror(-err) << endl << flush; cerr << "Error task create: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
@ -218,7 +214,7 @@ 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_cameraOpen, (void(*)(void*)) & Tasks::OpenCamera, this)) { if (err = rt_task_start(&th_cameraManage, (void(*)(void*)) & Tasks::ManageCamera, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush; cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
} }
@ -226,10 +222,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_cameraClose, (void(*)(void*)) & Tasks::CloseCamera, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE);
}
if (err = rt_task_start(&th_arenaChoice, (void(*)(void*)) & Tasks::ArenaChoice, this)) { if (err = rt_task_start(&th_arenaChoice, (void(*)(void*)) & Tasks::ArenaChoice, this)) {
cerr << "Error task start: " << strerror(-err) << endl << flush; cerr << "Error task start: " << strerror(-err) << endl << flush;
exit(EXIT_FAILURE); exit(EXIT_FAILURE);
@ -370,14 +362,14 @@ void Tasks::ReceiveFromMonTask(void *arg) {
else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) { else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) {
std::cout << "\n\n\nArena Confirm asked !!!\n\n\n" << std::endl; std::cout << "\n\n\nArena Confirm asked !!!\n\n\n" << std::endl;
rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE); rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
if(cameraStatus == ArenaStatusEnum::SEARCHING) if(cameraStatus == ArenaStatusEnum::SEARCHED)
cameraStatus = ArenaStatusEnum::CONFIRM; cameraStatus = ArenaStatusEnum::CONFIRM;
rt_mutex_release(&mutex_arenaStatus); rt_mutex_release(&mutex_arenaStatus);
} }
else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) { else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) {
std::cout << "\n\n\nArena Infirm asked !!!\n\n\n" << std::endl; std::cout << "\n\n\nArena Infirm asked !!!\n\n\n" << std::endl;
rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE); rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
if(cameraStatus == ArenaStatusEnum::SEARCHING) if(cameraStatus == ArenaStatusEnum::SEARCHED)
cameraStatus = ArenaStatusEnum::INFIRM; cameraStatus = ArenaStatusEnum::INFIRM;
rt_mutex_release(&mutex_arenaStatus); rt_mutex_release(&mutex_arenaStatus);
} }
@ -561,7 +553,7 @@ void Tasks::BatteryStatusTask(void * arg) {
} }
} }
void Tasks::OpenCamera(void * arg) void Tasks::ManageCamera(void * arg)
{ {
// Variables // Variables
int rs(0); int rs(0);
@ -583,26 +575,23 @@ void Tasks::OpenCamera(void * arg)
rs = robotStarted; rs = robotStarted;
rt_mutex_release(&mutex_robotStarted); rt_mutex_release(&mutex_robotStarted);
if(rs != 0)
{
// Check the status of the camera // Check the status of the camera
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE); rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);
cs = cameraStatus; cs = cameraStatus;
rt_mutex_release(&mutex_cameraStatus); rt_mutex_release(&mutex_cameraStatus);
if((rs != 0) && (cs == CameraStatusEnum::OPENING))
if(cs == CameraStatusEnum::OPENING)
{ {
rt_mutex_acquire(&mutex_camera, TM_INFINITE); const MessageID tempMessage = this->OpenCamera();
cam = new Camera(sm, 10); WriteInQueue(&q_messageToMon, new Message(tempMessage));
bool is_opened = cam->Open(); }
rt_mutex_release(&mutex_camera); if(cs == CameraStatusEnum::CLOSING)
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);
if(is_opened)
{ {
cameraStatus = CameraStatusEnum::OPENED; this->CloseCamera();
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK)); WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
} }
else {
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_NACK));
}
rt_mutex_release(&mutex_cameraStatus);
} }
} }
} }
@ -621,62 +610,73 @@ void Tasks::ImageCamera(void * arg)
while(1) while(1)
{ {
rt_task_wait_period(NULL); rt_task_wait_period(NULL);
// Check the status of the camera // Check the status of the camera
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE); rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);
cs = cameraStatus; cs = cameraStatus;
rt_mutex_release(&mutex_cameraStatus); rt_mutex_release(&mutex_cameraStatus);
// Checking if camera is opened first
if(cs == CameraStatusEnum::OPENED) if(cs == CameraStatusEnum::OPENED)
{ {
// Gathering image from camera
Img * img = new Img(cam->Grab()); Img * img = new Img(cam->Grab());
// Sending Image
MessageImg *msgImg = new MessageImg(MESSAGE_CAM_IMAGE, img); MessageImg *msgImg = new MessageImg(MESSAGE_CAM_IMAGE, img);
rt_mutex_acquire(&mutex_monitor, TM_INFINITE); rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
monitor.Write(msgImg); monitor.Write(msgImg);
rt_mutex_release(&mutex_monitor); rt_mutex_release(&mutex_monitor);
// ? delete img;
} }
} }
} }
void Tasks::CloseCamera(void * arg) MessageID Tasks::OpenCamera()
{ {
// Variables cout << "Called " << __PRETTY_FUNCTION__ << endl << flush;
CameraStatusEnum cs(CameraStatusEnum::CLOSED); // Opening camera
rt_mutex_acquire(&mutex_camera, TM_INFINITE);
cam = new Camera(sm, 10);
const bool isOpened = cam->Open();
rt_mutex_release(&mutex_camera);
cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Changing status
// Synchronization barrier (waiting that all tasks are starting) if(isOpened)
rt_sem_p(&sem_barrier, TM_INFINITE);
rt_task_set_periodic(NULL, TM_NOW, 500000000);
while(1)
{ {
rt_task_wait_period(NULL);
// Check the status of the camera
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE); rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);
cs = cameraStatus; cameraStatus = CameraStatusEnum::OPENED;
rt_mutex_release(&mutex_cameraStatus); rt_mutex_release(&mutex_cameraStatus);
if(cs == CameraStatusEnum::CLOSING) }
return (is_opened ? MESSAGE_ANSWER_ACK : MESSAGE_ANSWER_NACK);
}
void Tasks::CloseCamera()
{ {
cout << "Called " << __PRETTY_FUNCTION__ << endl << flush;
// Closing Camera
rt_mutex_acquire(&mutex_camera, TM_INFINITE); rt_mutex_acquire(&mutex_camera, TM_INFINITE);
cam->Close(); cam->Close();
delete cam; delete cam;
rt_mutex_release(&mutex_camera); rt_mutex_release(&mutex_camera);
// Changing Status
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE); rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);
cameraStatus = CameraStatusEnum::CLOSED; cameraStatus = CameraStatusEnum::CLOSED;
rt_mutex_release(&mutex_cameraStatus); rt_mutex_release(&mutex_cameraStatus);
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
}
}
} }
void Tasks::ArenaChoice(void * arg) void Tasks::ArenaChoice(void * arg)
{ {
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);
// Variables
Img * img = nullptr; Img * img = nullptr;
Arena a; Arena a;
ArenaStatusEnum as = ArenaStatusEnum::NONE; ArenaStatusEnum as(ArenaStatusEnum::NONE);
while(1) while(1)
{ {
@ -688,14 +688,16 @@ void Tasks::ArenaChoice(void * arg)
// ASK_ARENA // ASK_ARENA
if(as == ArenaStatusEnum::SEARCHING) if(as == ArenaStatusEnum::SEARCHING)
{ {
rt_mutex_acquire(&mutex_cameraStatus, TM_INFINITE);
const CameraStatusEnum cs = cameraStatus;
rt_mutex_release(&mutex_cameraStatus);
// Gathering last image + closing camera when prompted // Gathering last image + closing camera when prompted
rt_mutex_acquire(&mutex_camera, TM_INFINITE); if(CameraStatusEnum::OPENED == cs)
if(cam->IsOpen())
{ {
img = new Img(cam->Grab()); new Img(cam->Grab());
cam->Close(); this->CloseCamera();
} }
rt_mutex_release(&mutex_camera);
// Putting the arena overlay if found // Putting the arena overlay if found
a = img->SearchArena(); a = img->SearchArena();
@ -708,24 +710,34 @@ void Tasks::ArenaChoice(void * arg)
monitor.Write(msgImg); monitor.Write(msgImg);
rt_mutex_release(&mutex_monitor); rt_mutex_release(&mutex_monitor);
} }
// ? delete img;
} }
// ARENA_CONFIRM / INFIRM // ARENA_CONFIRM / INFIRM
else if(as != ArenaStatusEnum::NONE) else if(as == ArenaStatusEnum::CONFIRM || as == ArenaStatusEnum::INFIRM)
{ {
// Store arena to internal buffer + change status
if(as == ArenaStatusEnum::CONFIRM) if(as == ArenaStatusEnum::CONFIRM)
{ {
rt_mutex_acquire(&mutex_arenaList, TM_INFINITE); rt_mutex_acquire(&mutex_arena, TM_INFINITE);
arenaList.emplace_back(a); arena = a;
rt_mutex_release(&mutex_arenaList); rt_mutex_release(&mutex_arena);
rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
arenaStatus = ArenaStatusEnum::CONFIRMED;
rt_mutex_release(&mutex_arenaStatus);
} }
// Re-open the camera // Arena Status set to NONE to enable new search
rt_mutex_acquire(&mutex_camera, TM_INFINITE); else {
cam->Open(); rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
rt_mutex_release(&mutex_camera); arenaStatus = ArenaStatusEnum::NONE;
// empty the arena and write ACK to the monitor rt_mutex_release(&mutex_arenaStatus);
}
// empty the temporary arena
a = Arena(); a = Arena();
WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK)); // Re-open the camera
const MessageID tempMessage = this->OpenCamera();
WriteInQueue(&q_messageToMon, new Message(tempMessage));
} }
} }
} }

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

@ -47,7 +47,9 @@ enum class CameraStatusEnum {
enum class ArenaStatusEnum { enum class ArenaStatusEnum {
NONE, NONE,
SEARCHING, SEARCHING,
SEARCHED,
CONFIRM, CONFIRM,
CONFIRMED,
INFIRM INFIRM
}; };
@ -84,7 +86,7 @@ private:
bool robotBatteryGet = false; bool robotBatteryGet = false;
CameraStatusEnum cameraStatus = CameraStatusEnum::CLOSED; CameraStatusEnum cameraStatus = CameraStatusEnum::CLOSED;
ArenaStatusEnum arenaStatus = ArenaStatusEnum::NONE; ArenaStatusEnum arenaStatus = ArenaStatusEnum::NONE;
std::vector<Arena> arenaList; Arena arena;
Camera* cam = nullptr; Camera* cam = nullptr;
/**********************************************************************/ /**********************************************************************/
@ -97,9 +99,8 @@ private:
RT_TASK th_startRobot; RT_TASK th_startRobot;
RT_TASK th_move; RT_TASK th_move;
RT_TASK th_battery; RT_TASK th_battery;
RT_TASK th_cameraOpen; RT_TASK th_cameraManage;
RT_TASK th_cameraImage; RT_TASK th_cameraImage;
RT_TASK th_cameraClose;
RT_TASK th_arenaChoice; RT_TASK th_arenaChoice;
/**********************************************************************/ /**********************************************************************/
@ -112,7 +113,7 @@ private:
RT_MUTEX mutex_batteryGet; RT_MUTEX mutex_batteryGet;
RT_MUTEX mutex_cameraStatus; RT_MUTEX mutex_cameraStatus;
RT_MUTEX mutex_camera; RT_MUTEX mutex_camera;
RT_MUTEX mutex_arenaList; RT_MUTEX mutex_arena;
RT_MUTEX mutex_arenaStatus; RT_MUTEX mutex_arenaStatus;
/**********************************************************************/ /**********************************************************************/
@ -165,13 +166,14 @@ private:
/* OUR CODE */ /* OUR CODE */
// Threads
void BatteryStatusTask(void * arg); void BatteryStatusTask(void * arg);
void OpenCamera(void * arg); void ManageCamera(void * arg);
void ImageCamera(void * arg); void ImageCamera(void * arg);
void CloseCamera(void * arg);
void ArenaChoice(void * arg); void ArenaChoice(void * arg);
// Utility functions // Utility functions
MessageID OpenCamera();
void CloseCamera();
/**********************************************************************/ /**********************************************************************/