diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp index 3e5810c..70ea1ca 100644 --- a/software/raspberry/superviseur-robot/tasks.cpp +++ b/software/raspberry/superviseur-robot/tasks.cpp @@ -59,81 +59,81 @@ void Tasks::Init() { * Mutex creation * *************************************************************************************/ if ((err = rt_mutex_create(&mutex_monitor, nullptr))) { - cerr << "Error mutex create: " << strerror(-err) << endl << flush; + cerr << "Error mutex create mutex_monitor: " << strerror(-err) << endl; exit(EXIT_FAILURE); } if ((err = rt_mutex_create(&mutex_robot, nullptr))) { - cerr << "Error mutex create: " << strerror(-err) << endl << flush; + cerr << "Error mutex create mutex_robot: " << strerror(-err) << endl; exit(EXIT_FAILURE); } if ((err = rt_mutex_create(&mutex_robotStarted, nullptr))) { - cerr << "Error mutex create: " << strerror(-err) << endl << flush; + cerr << "Error mutex create mutex_robotStarted: " << strerror(-err) << endl; exit(EXIT_FAILURE); } if ((err = rt_mutex_create(&mutex_move, nullptr))) { - cerr << "Error mutex create: " << strerror(-err) << endl << flush; + cerr << "Error mutex create mutex_move: " << strerror(-err) << endl; exit(EXIT_FAILURE); } - cout << "Mutexes created successfully" << endl << flush; + cout << "Mutexes created successfully" << endl; /* ************************************************************************************** * Semaphores creation * *************************************************************************************/ if ((err = rt_sem_create(&sem_barrier, nullptr, 0, S_FIFO))) { - cerr << "Error semaphore create: " << strerror(-err) << endl << flush; + cerr << "Error semaphore create sem_barrier: " << strerror(-err) << endl; exit(EXIT_FAILURE); } if ((err = rt_sem_create(&sem_openComRobot, nullptr, 0, S_FIFO))) { - cerr << "Error semaphore create: " << strerror(-err) << endl << flush; + cerr << "Error semaphore create sem_openComRobot: " << strerror(-err) << endl; exit(EXIT_FAILURE); } if ((err = rt_sem_create(&sem_serverOk, nullptr, 0, S_FIFO))) { - cerr << "Error semaphore create: " << strerror(-err) << endl << flush; + cerr << "Error semaphore create sem_serverOk: " << strerror(-err) << endl; exit(EXIT_FAILURE); } if ((err = rt_sem_create(&sem_startRobot, nullptr, 0, S_FIFO))) { - cerr << "Error semaphore create: " << strerror(-err) << endl << flush; + cerr << "Error semaphore create sem_startRobot: " << strerror(-err) << endl; exit(EXIT_FAILURE); } - cout << "Semaphores created successfully" << endl << flush; + cout << "Semaphores created successfully" << endl; /* ************************************************************************************* * Tasks creation * *************************************************************************************/ if ((err = rt_task_create(&th_server, "th_server", 0, PRIORITY_TSERVER, 0))) { - cerr << "Error task create: " << strerror(-err) << endl << flush; + cerr << "Error task create th_server: " << strerror(-err) << endl; exit(EXIT_FAILURE); } if ((err = rt_task_create(&th_sendToMon, "th_sendToMon", 0, PRIORITY_TSENDTOMON, 0))) { - cerr << "Error task create: " << strerror(-err) << endl << flush; + cerr << "Error task create th_sendToMon: " << strerror(-err) << endl; exit(EXIT_FAILURE); } if ((err = rt_task_create(&th_receiveFromMon, "th_receiveFromMon", 0, PRIORITY_TRECEIVEFROMMON, 0))) { - cerr << "Error task create: " << strerror(-err) << endl << flush; + cerr << "Error task create th_receiveFromMon: " << strerror(-err) << endl; exit(EXIT_FAILURE); } if ((err = rt_task_create(&th_openComRobot, "th_openComRobot", 0, PRIORITY_TOPENCOMROBOT, 0))) { - cerr << "Error task create: " << strerror(-err) << endl << flush; + cerr << "Error task create th_openComRobot: " << strerror(-err) << endl; exit(EXIT_FAILURE); } - if (err == rt_task_create(&th_startRobot, "th_startRobot", 0, PRIORITY_TSTARTROBOT, 0)) { - cerr << "Error task create: " << strerror(-err) << endl << flush; + if ((err = rt_task_create(&th_startRobot, "th_startRobot", 0, PRIORITY_TSTARTROBOT, 0))) { + cerr << "Error task create th_startRobot: " << strerror(-err) << endl; exit(EXIT_FAILURE); } if ((err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0))) { - cerr << "Error task create: " << strerror(-err) << endl << flush; + cerr << "Error task create th_move: " << strerror(-err) << endl; exit(EXIT_FAILURE); } - cout << "Tasks created successfully" << endl << flush; + cout << "Tasks created successfully" << endl; /* ************************************************************************************* * Message queues creation * *************************************************************************************/ if ((err = rt_queue_create(&q_messageToMon, "q_messageToMon", sizeof (Message*)*50, Q_UNLIMITED, Q_FIFO)) < 0) { - cerr << "Error msg queue create: " << strerror(-err) << endl << flush; + cerr << "Error msg queue create q_messageToMon: " << strerror(-err) << endl; exit(EXIT_FAILURE); } - cout << "Queues created successfully" << endl << flush; + cout << "Queues created successfully" << endl; } @@ -145,31 +145,31 @@ void Tasks::Run() { int err; if (err = rt_task_start(&th_server, (void(*)(void*)) & Tasks::ServerTask, this)) { - cerr << "Error task start: " << strerror(-err) << endl << flush; + cerr << "Error task start ServerTask: " << strerror(-err) << endl; exit(EXIT_FAILURE); } if (err = rt_task_start(&th_sendToMon, (void(*)(void*)) & Tasks::SendToMonTask, this)) { - cerr << "Error task start: " << strerror(-err) << endl << flush; + cerr << "Error task start SendToMonTask: " << strerror(-err) << endl; exit(EXIT_FAILURE); } if (err = rt_task_start(&th_receiveFromMon, (void(*)(void*)) & Tasks::ReceiveFromMonTask, this)) { - cerr << "Error task start: " << strerror(-err) << endl << flush; + cerr << "Error task start ReceiveFromMonTask: " << strerror(-err) << endl; exit(EXIT_FAILURE); } if (err = rt_task_start(&th_openComRobot, (void(*)(void*)) & Tasks::OpenComRobot, this)) { - cerr << "Error task start: " << strerror(-err) << endl << flush; + cerr << "Error task start OpenComRobot: " << strerror(-err) << endl; exit(EXIT_FAILURE); } if (err = rt_task_start(&th_startRobot, (void(*)(void*)) & Tasks::StartRobotTask, this)) { - cerr << "Error task start: " << strerror(-err) << endl << flush; + cerr << "Error task start StartRobotTask: " << strerror(-err) << endl; exit(EXIT_FAILURE); } if (err = rt_task_start(&th_move, (void(*)(void*)) & Tasks::MoveTask, this)) { - cerr << "Error task start: " << strerror(-err) << endl << flush; + cerr << "Error task start MoveTask: " << strerror(-err) << endl; exit(EXIT_FAILURE); } - cout << "Tasks launched" << endl << flush; + cout << "Tasks launched" << endl; } /** @@ -183,7 +183,7 @@ void Tasks::Stop() { /** */ void Tasks::Join() { - cout << "Tasks synchronized" << endl << flush; + cout << "Tasks synchronized" << endl; rt_sem_broadcast(&sem_barrier); pause(); } @@ -194,7 +194,7 @@ void Tasks::Join() { void Tasks::ServerTask(void *arg) { int status; - cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; + cout << "Start " << __PRETTY_FUNCTION__ << endl; // Synchronization barrier (waiting that all tasks are started) rt_sem_p(&sem_barrier, TM_INFINITE); @@ -211,7 +211,7 @@ void Tasks::ServerTask(void *arg) { "Unable to start server on port " + std::to_string(SERVER_PORT) }; monitor.AcceptClient(); // Wait the monitor client - cout << "Rock'n'Roll baby, client accepted!" << endl << flush; + cout << "Rock'n'Roll baby, client accepted!" << endl; rt_sem_broadcast(&sem_serverOk); } @@ -221,7 +221,7 @@ void Tasks::ServerTask(void *arg) { [[noreturn]] void Tasks::SendToMonTask(void* arg) { Message *msg; - cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; + cout << "Start " << __PRETTY_FUNCTION__ << endl; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); @@ -231,9 +231,9 @@ void Tasks::ServerTask(void *arg) { rt_sem_p(&sem_serverOk, TM_INFINITE); while (true) { - cout << "wait msg to send" << endl << flush; + cout << "wait msg to send" << endl; msg = ReadInQueue(&q_messageToMon); - cout << "Send msg to mon: " << msg->ToString() << endl << flush; + cout << "Send msg to mon: " << msg->ToString() << endl; rt_mutex_acquire(&mutex_monitor, TM_INFINITE); monitor.Write(msg); // The message is deleted with the Write rt_mutex_release(&mutex_monitor); @@ -246,7 +246,7 @@ void Tasks::ServerTask(void *arg) { void Tasks::ReceiveFromMonTask(void *arg) { Message *msgRcv; - cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; + cout << "Start " << __PRETTY_FUNCTION__ << endl; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); @@ -254,11 +254,11 @@ void Tasks::ReceiveFromMonTask(void *arg) { * The task receiveFromMon starts here * *************************************************************************************/ rt_sem_p(&sem_serverOk, TM_INFINITE); - cout << "Received message from monitor activated" << endl << flush; + cout << "Received message from monitor activated" << endl; while (true) { msgRcv = monitor.Read(); - cout << "Rcv <= " << msgRcv->ToString() << endl << flush; + cout << "Rcv <= " << msgRcv->ToString() << endl; if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) { delete(msgRcv); @@ -288,7 +288,7 @@ void Tasks::ReceiveFromMonTask(void *arg) { int status; int err; - cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; + cout << "Start " << __PRETTY_FUNCTION__ << endl; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); @@ -318,7 +318,7 @@ void Tasks::ReceiveFromMonTask(void *arg) { * @brief Thread starting the communication with the robot. */ [[noreturn]] void Tasks::StartRobotTask(void *arg) { - cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; + cout << "Start " << __PRETTY_FUNCTION__ << endl; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); @@ -335,7 +335,7 @@ void Tasks::ReceiveFromMonTask(void *arg) { cout << msgSend->GetID(); cout << ")" << endl; - cout << "Movement answer: " << msgSend->ToString() << endl << flush; + cout << "Movement answer: " << msgSend->ToString() << endl; WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { @@ -353,7 +353,7 @@ void Tasks::ReceiveFromMonTask(void *arg) { int rs; int cpMove; - cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; + cout << "Start " << __PRETTY_FUNCTION__ << endl; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); @@ -391,7 +391,7 @@ void Tasks::ReceiveFromMonTask(void *arg) { void Tasks::WriteInQueue(RT_QUEUE *queue, Message *msg) { int err; if ((err = rt_queue_write(queue, (const void *) &msg, sizeof ((const void *) &msg), Q_NORMAL)) < 0) { - cerr << "Write in queue failed: " << strerror(-err) << endl << flush; + cerr << "Write in queue failed: " << strerror(-err) << endl; throw std::runtime_error{"Error in write in queue"}; } } @@ -406,7 +406,7 @@ Message *Tasks::ReadInQueue(RT_QUEUE *queue) { Message *msg; if ((err = rt_queue_read(queue, &msg, sizeof ((void*) &msg), TM_INFINITE)) < 0) { - cout << "Read in queue failed: " << strerror(-err) << endl << flush; + cout << "Read in queue failed: " << strerror(-err) << endl; throw std::runtime_error{"Error in read in queue"}; }/** else { cout << "@msg :" << msg << endl << flush;