/* * Copyright (C) 2018 dimercur * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include "tasks.h" #include // Déclaration des priorités des taches #define PRIORITY_TSERVER 30 #define PRIORITY_TOPENCOMROBOT 50 #define PRIORITY_TMOVE 20 #define PRIORITY_TSENDTOMON 22 #define PRIORITY_TRECEIVEFROMMON 25 #define PRIORITY_TSTARTROBOTWITHOUTWATCHDOG 22 #define PRIORITY_TSTARTROBOTWITHWATCHDOG 22 #define PRIORITY_TCAMERA 21 #define PRIORITY_DETECTLOSTSUPROB 21 /* * Some remarks: * 1- This program is mostly a template. It shows you how to create tasks, semaphore * message queues, mutex ... and how to use them * * 2- semDumber is, as name say, useless. Its goal is only to show you how to use semaphore * * 3- Data flow is probably not optimal * * 4- Take into account that ComRobot::Write will block your task when serial buffer is full, * time for internal buffer to flush * * 5- Same behavior existe for ComMonitor::Write ! * * 6- When you want to write something in terminal, use cout and terminate with endl and flush * * 7- Good luck ! */ /** * @brief Initialisation des structures de l'application (tâches, mutex, * semaphore, etc.) */ void Tasks::Init() { int status; int err; /**************************************************************************************/ /* Mutex creation */ /**************************************************************************************/ if (err = rt_mutex_create(&mutex_monitor, NULL)) { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutex_robot, NULL)) { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutex_robotStarted, NULL)) { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutex_move, NULL)) { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutex_killReceiveFromMon, NULL)) { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutex_killVision, NULL)) { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutex_searchArena, NULL)) { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutex_drawArena, NULL)) { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutex_getPosition, NULL)) { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutex_killSendMon, NULL)) { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutex_killDetectLostSupRob, NULL)) { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutex_acquireImage, NULL)) { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_mutex_create(&mutex_killOpenComRobot, NULL)) { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } /*if (err = rt_mutex_create(&mutex_robot_on, NULL)) { cerr << "Error mutex create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); }*/ cout << "Mutexes created successfully" << endl << flush; /**************************************************************************************/ /* Semaphors creation */ /**************************************************************************************/ if (err = rt_sem_create(&sem_barrier, NULL, 0, S_FIFO)) { cerr << "Error semaphore create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_sem_create(&sem_openComRobot, NULL, 0, S_FIFO)) { cerr << "Error semaphore create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_sem_create(&sem_serverOk, NULL, 0, S_FIFO)) { cerr << "Error semaphore create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_sem_create(&sem_startRobotWithoutWatchdog, NULL, 0, S_FIFO)) { cerr << "Error semaphore create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_sem_create(&sem_startRobotWithWatchdog, NULL, 0, S_FIFO)) { cerr << "Error semaphore create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_sem_create(&sem_detectLostSupRob, NULL, 0, S_FIFO)) { cerr << "Error semaphore create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_sem_create(&sem_restart, NULL, 0, S_FIFO)) { cerr << "Error semaphore create: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } cout << "Semaphores created successfully" << endl << flush; /**************************************************************************************/ /* Tasks creation */ /**************************************************************************************/ if (err = rt_task_create(&th_server, "th_server", 0, PRIORITY_TSERVER, 0)) { cerr << "Error task create server: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_task_create(&th_sendToMon, "th_sendToMon", 0, PRIORITY_TSENDTOMON, 0)) { cerr << "Error task create sendtoMon: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_task_create(&th_receiveFromMon, "th_receiveFromMon", 0, PRIORITY_TRECEIVEFROMMON, 0)) { cerr << "Error task create receiveFromMon: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_task_create(&th_openComRobot, "th_openComRobot", 0, PRIORITY_TOPENCOMROBOT, 0)) { cerr << "Error task create openComRobot: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_task_create(&th_startRobotWithoutWatchdog, "th_startRobotWithoutWatchdog", 0, PRIORITY_TSTARTROBOTWITHOUTWATCHDOG, 0)) { cerr << "Error task create startRobotWithoutWatchdog: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_task_create(&th_startRobotWithWatchdog, "th_startRobotWithWatchdog", 0, PRIORITY_TSTARTROBOTWITHWATCHDOG, 0)) { cerr << "Error task create startRobotWithWatchdog: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0)) { cerr << "Error task create move: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_task_create(&th_detectLostSupRob, "th_detectLostSupRob", 0, PRIORITY_DETECTLOSTSUPROB, 0)) { cerr << "Error task create detectLostSupRob: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } cout << "Tasks created successfully" << endl << flush; /**************************************************************************************/ /* 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; exit(EXIT_FAILURE); } cout << "Queues created successfully" << endl << flush; } /** * @brief Démarrage des tâches */ void Tasks::Run() { cout << "Coucou" << endl << flush; rt_task_set_priority(NULL, T_LOPRIO); int err; if (err = rt_task_start(&th_server, (void(*)(void*)) & Tasks::ServerTask, this)) { cerr << "Error task start server: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_task_start(&th_sendToMon, (void(*)(void*)) & Tasks::SendToMonTask, this)) { cerr << "Error task start sendToMon: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_task_start(&th_receiveFromMon, (void(*)(void*)) & Tasks::ReceiveFromMonTask, this)) { cerr << "Error task start receiveFromMon: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_task_start(&th_openComRobot, (void(*)(void*)) & Tasks::OpenComRobot, this)) { cerr << "Error task start openComRobot: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_task_start(&th_startRobotWithoutWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithoutWatchdog, this)) { cerr << "Error task start startRobotWithoutWatchdog: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_task_start(&th_startRobotWithWatchdog, (void(*)(void*)) & Tasks::StartRobotTaskWithWatchdog, this)) { cerr << "Error task start startRobotWithWatchdog: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_task_start(&th_move, (void(*)(void*)) & Tasks::MoveTask, this)) { cerr << "Error task start move: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } if (err = rt_task_start(&th_detectLostSupRob, (void(*)(void*)) & Tasks::DetectLostSupRob, this)) { cerr << "Error task start detectLostSupRob: " << strerror(-err) << endl << flush; exit(EXIT_FAILURE); } cout << "Tasks launched" << endl << flush; } /** * @brief Arrêt des tâches */ void Tasks::Stop() { monitor.Close(); robot.Close(); } /** */ void Tasks::Join() { cout << "Tasks synchronized" << endl << flush; rt_sem_broadcast(&sem_barrier); //Initialization for some specific sem: Allow to run the first time //rt_sem_broadcast(&sem_restart); pause(); } /** * @brief Thread handling server communication with the monitor. */ void Tasks::ServerTask(void *arg) { int status; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; while(1){ // Synchronization barrier (waiting that all tasks are started) rt_sem_p(&sem_barrier, TM_INFINITE); /**************************************************************************************/ /* The task server starts here */ /**************************************************************************************/ rt_mutex_acquire(&mutex_monitor, TM_INFINITE); status = monitor.Open(SERVER_PORT); rt_mutex_release(&mutex_monitor); cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl; if (status < 0) throw std::runtime_error { "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; rt_sem_broadcast(&sem_serverOk); } } /** * @brief Thread sending data to monitor. */ void Tasks::SendToMonTask(void* arg) { Message *msg; bool kill_sendToMonOk=0; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; while(1){ // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); /**************************************************************************************/ /* The task sendToMon starts here */ /**************************************************************************************/ rt_sem_p(&sem_serverOk, TM_INFINITE); //Initialize the loop condition kill_sendToMonOk=0; rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE); killSendMon=0; // The message is deleted with the Write rt_mutex_release(&mutex_killSendMon); while (!kill_sendToMonOk) { cout << "wait msg to send" << endl << flush; msg = ReadInQueue(&q_messageToMon); 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); rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE); kill_sendToMonOk=killSendMon; // The message is deleted with the Write rt_mutex_release(&mutex_killSendMon); } cout << "SendToMon Task dies" << endl << flush; } } /** * @brief Thread receiving data from monitor. */ void Tasks::ReceiveFromMonTask(void *arg) { Message *msgRcv; bool killReceiveFromMonOk=0; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); //while(1){ //rt_sem_p(&sem_restart,TM_INFINITE); //Reinitialize control boolean rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE); killReceiveFromMon=0; rt_mutex_release(&mutex_killReceiveFromMon); /**************************************************************************************/ /* The task receiveFromMon starts here */ /**************************************************************************************/ rt_sem_p(&sem_serverOk, TM_INFINITE); cout << "Received message from monitor activated" << endl << flush; while (!killReceiveFromMonOk) { msgRcv = monitor.Read(); cout << "Rcv <= " << msgRcv->ToString() << endl << flush; if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) { delete(msgRcv); cout << "Connection to monitor lost" << endl; monitor.Close(); rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE); killReceiveFromMon=1; rt_mutex_release(&mutex_killReceiveFromMon); rt_mutex_acquire(&mutex_killVision, TM_INFINITE); killVision=1; rt_mutex_release(&mutex_killVision); rt_mutex_acquire(&mutex_killSendMon, TM_INFINITE); killSendMon=1; rt_mutex_release(&mutex_killSendMon); //Write fake message in queue to unblock Read function WriteInQueue(&q_messageToMon, new Message(MESSAGE_EMPTY)); rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); killBattery=1; rt_mutex_release(&mutex_killBattery); rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE); killDetectLostSupRob=1; rt_mutex_release(&mutex_killDetectLostSupRob); rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); acquireImage=0; rt_mutex_release(&mutex_acquireImage); //close Monitor monitor.Close(); robot.Close(); sleep(2); //Restart all the process Tasks::Join(); //rt_sem_v(&sem_restart); //exit(-1); } else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) { cout << "Command Open Camera Received" << endl << flush; //start task Vision } else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) { cout << "Command Close Camera Received" << endl << flush; //Trigger killVision rt_mutex_acquire(&mutex_killVision, TM_INFINITE); killVision=1; rt_mutex_release(&mutex_killVision); } else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) { cout << "Command Open Communication with Robot Received" << endl << flush; rt_sem_v(&sem_openComRobot); } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) { cout << "Command Start Robot without Watchdog Received" << endl << flush; rt_sem_v(&sem_startRobotWithoutWatchdog); } else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) { cout << "Command Start Robot with Watchdog Received" << endl << flush; //start task robot with watchdog } else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) { cout << "Command Search Arena Received" << endl << flush; rt_mutex_acquire(&mutex_searchArena, TM_INFINITE); searchArena=1; rt_mutex_release(&mutex_searchArena); } else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) { cout << "Command Get Robot Position Received" << endl << flush; rt_mutex_acquire(&mutex_getPosition, TM_INFINITE); getPosition=1; rt_mutex_release(&mutex_getPosition); } else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)) { cout << "Command Stop Getting Robot Position Received" << endl << flush; rt_mutex_acquire(&mutex_getPosition, TM_INFINITE); getPosition=0; rt_mutex_release(&mutex_getPosition); } else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) { cout << "Command Confirm Arena Received" << endl << flush; rt_mutex_acquire(&mutex_drawArena, TM_INFINITE); drawArena=1; rt_mutex_release(&mutex_drawArena); rt_mutex_acquire(&mutex_searchArena, TM_INFINITE); searchArena=0; rt_mutex_release(&mutex_searchArena); rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); acquireImage=1; rt_mutex_release(&mutex_acquireImage); } else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) { cout << "Command Infirm Arena Received" << endl << flush; rt_mutex_acquire(&mutex_searchArena, TM_INFINITE); searchArena=0; rt_mutex_release(&mutex_searchArena); rt_mutex_acquire(&mutex_acquireImage, TM_INFINITE); acquireImage=1; rt_mutex_release(&mutex_acquireImage); } else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) || msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) || msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) || msgRcv->CompareID(MESSAGE_ROBOT_GO_RIGHT) || msgRcv->CompareID(MESSAGE_ROBOT_STOP)) { rt_mutex_acquire(&mutex_move, TM_INFINITE); move = msgRcv->GetID(); rt_mutex_release(&mutex_move); } delete(msgRcv); // must be deleted manually, no consumer //Update loop condition rt_mutex_acquire(&mutex_killReceiveFromMon, TM_INFINITE); killReceiveFromMonOk=killReceiveFromMon; rt_mutex_release(&mutex_killReceiveFromMon); } //} } /** * @brief Thread opening communication with the robot. */ void Tasks::OpenComRobot(void *arg) { int status; int err; //bool killOpenComRobotOk = 0; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); /**************************************************************************************/ /* The task openComRobot starts here */ /**************************************************************************************/ while (1) { rt_sem_p(&sem_openComRobot, TM_INFINITE); cout << "Open serial com ("; rt_mutex_acquire(&mutex_robot, TM_INFINITE); status = robot.Open(); rt_mutex_release(&mutex_robot); cout << status; cout << ")" << endl << flush; Message * msgSend; if (status < 0) { msgSend = new Message(MESSAGE_ANSWER_NACK); } else { msgSend = new Message(MESSAGE_ANSWER_ACK); } WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon //Trigger Detection of Communication Loss with Robot rt_sem_v(&sem_detectLostSupRob); } } /** * @brief Thread starting the communication with the robot. *//** * @brief Thread starting the communication with the robot. */ void Tasks::StartRobotTaskWithoutWatchdog(void *arg) { cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); int killBattery=0; /**************************************************************************************/ /* The task startRobot starts here */ /**************************************************************************************/ Message* p_mess_answer_battery; Message * msgSend; rt_sem_p(&sem_startRobotWithoutWatchdog, TM_INFINITE); cout << "Start robot without watchdog ("; //Boolean to get the battery rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); killBattery=0; rt_mutex_release(&mutex_killBattery); rt_mutex_acquire(&mutex_robot, TM_INFINITE); msgSend = robot.Write(robot.StartWithoutWD()); rt_mutex_release(&mutex_robot); cout << msgSend->GetID(); cout << ")" << endl; cout << "Movement answer: " << msgSend->ToString() << endl << flush; WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); robotStarted = 1; rt_mutex_release(&mutex_robotStarted); rt_task_set_periodic(NULL, TM_NOW, 500000000); while (killBattery==0) { rt_task_wait_period(NULL); rt_mutex_acquire(&mutex_robot, TM_INFINITE); p_mess_answer_battery = robot.Write(robot.GetBattery()); rt_mutex_release(&mutex_robot); rt_mutex_acquire(&mutex_monitor, TM_INFINITE); monitor.Write(p_mess_answer_battery); rt_mutex_release(&mutex_monitor); //cout << endl << flush; rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); killBattery=killBattery; rt_mutex_release(&mutex_killBattery); } } } /** * @brief Thread starting the communication with the robot. *//** * @brief Thread starting the communication with the robot. */ void Tasks::StartRobotTaskWithWatchdog(void *arg) { cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); /**************************************************************************************/ /* The task startRobot starts here */ /**************************************************************************************/ Message* p_mess_answer_battery; Message * msgSend; int cpt=1; int err; while(1){ rt_sem_p(&sem_restart, TM_INFINITE); rt_sem_p(&sem_startRobotWithWatchdog, TM_INFINITE); cout << "Start robot with watchdog ("; rt_mutex_acquire(&mutex_robot, TM_INFINITE); msgSend = robot.Write(robot.StartWithWD()); rt_mutex_release(&mutex_robot); //boolean to ask battery rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); killBattery=0; rt_mutex_release(&mutex_killBattery); cout << msgSend->GetID(); cout << ")" << endl; cout << "Movement answer: " << msgSend->ToString() << endl << flush; WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon if (msgSend->GetID() == MESSAGE_ANSWER_ACK) { rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); robotStarted = 1; rt_mutex_release(&mutex_robotStarted); rt_task_set_periodic(NULL, TM_NOW, 500000000); while (1) { cpt++; if(cpt%2==0){ rt_mutex_acquire(&mutex_robot, TM_INFINITE); robot.Write(robot.ReloadWD()); rt_mutex_release(&mutex_robot); } rt_task_wait_period(NULL); rt_mutex_acquire(&mutex_robot, TM_INFINITE); p_mess_answer_battery = robot.Write(robot.GetBattery()); rt_mutex_release(&mutex_robot); rt_mutex_acquire(&mutex_monitor, TM_INFINITE); monitor.Write(p_mess_answer_battery); rt_mutex_release(&mutex_monitor); cout << endl << flush; } } } } /** * @brief Thread handling control of the robot. */ void Tasks::MoveTask(void *arg) { int rs; int cpMove; cout << "Start " << __PRETTY_FUNCTION__ << endl << flush; // Synchronization barrier (waiting that all tasks are starting) rt_sem_p(&sem_barrier, TM_INFINITE); /**************************************************************************************/ /* The task starts here */ /**************************************************************************************/ rt_task_set_periodic(NULL, TM_NOW, 100000000); while (1) { rt_task_wait_period(NULL); cout << "Periodic movement update"; rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE); rs = robotStarted; rt_mutex_release(&mutex_robotStarted); if (rs == 1) { rt_mutex_acquire(&mutex_move, TM_INFINITE); cpMove = move; rt_mutex_release(&mutex_move); cout << " move: " << cpMove; rt_mutex_acquire(&mutex_robot, TM_INFINITE); robot.Write(new Message((MessageID)cpMove)); rt_mutex_release(&mutex_robot); } cout << endl << flush; } } void Tasks::DetectLostSupRob(void *arg){ //counter to detect loss of communication int cpt=0; bool kill_detectLostSupRobOk=0; Message* msgSend; //Wait the Communication with the Robot to be Set rt_sem_p(&sem_detectLostSupRob, TM_INFINITE); //Initialize the variable for the loop condition rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE); killDetectLostSupRob=0; rt_mutex_release(&mutex_killDetectLostSupRob); //Period = 1s rt_task_set_periodic(NULL, TM_NOW, 1000000000); while(!kill_detectLostSupRobOk){ rt_task_wait_period(NULL); //Test Communication with Robot rt_mutex_acquire(&mutex_robot, TM_INFINITE); msgSend = robot.Write(robot.Ping()); rt_mutex_release(&mutex_robot); if(msgSend->GetID() == MESSAGE_ANSWER_COM_ERROR || msgSend->GetID() == MESSAGE_ANSWER_ROBOT_TIMEOUT){ cout << "Didn't Get Any Answer from Robot" << endl << flush; cpt++; if(cpt==3){ //Trigger Kill of DetectLostSupRob rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE); killDetectLostSupRob=0; rt_mutex_release(&mutex_killDetectLostSupRob); //Trigger Kill of the Battery acquisition rt_mutex_acquire(&mutex_killBattery, TM_INFINITE); killBattery=0; rt_mutex_release(&mutex_killBattery); } } else{ cout << "I got an Answer from Robot" << endl << flush; } rt_mutex_acquire(&mutex_killDetectLostSupRob, TM_INFINITE); kill_detectLostSupRobOk=killDetectLostSupRob; rt_mutex_release(&mutex_killDetectLostSupRob); } } /** * Write a message in a given queue * @param queue Queue identifier * @param msg Message to be stored */ 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; throw std::runtime_error{"Error in write in queue"}; } } /** * Read a message from a given queue, block if empty * @param queue Queue identifier * @return Message read */ Message *Tasks::ReadInQueue(RT_QUEUE *queue) { int err; Message *msg; if ((err = rt_queue_read(queue, &msg, sizeof ((void*) &msg), TM_INFINITE)) < 0) { cout << "Read in queue failed: " << strerror(-err) << endl << flush; throw std::runtime_error{"Error in read in queue"}; }/** else { cout << "@msg :" << msg << endl << flush; } /**/ return msg; }