Improve receiveFromMon and serverTask

software/raspberry/superviseur-robot/lib/commonitor.cpp

@@ -100,7 +100,10 @@ int ComMonitor::Open(int port) {
         throw std::runtime_error{"Can not bind socket"};
     }
 
-    listen(socketFD, 1);
+    if (listen(socketFD, 1) < 0) {
+	    cerr<<"["<<__PRETTY_FUNCTION__<<"] Can not listen on port "<<to_string(port)<<endl<<flush;
+	    throw std::runtime_error{"Can not listen on this port"};
+    }
 
     return socketFD;
 }

software/raspberry/superviseur-robot/lib/commonitor.h

@@ -23,7 +23,7 @@
 
 using namespace std;
      
-#define SERVER_PORT 5544
+constexpr int SERVER_PORT = 5544;
 
 /**
  * Class used for generating a server and communicating through it with monitor

software/raspberry/superviseur-robot/tasks.cpp

@@ -220,9 +220,7 @@ void Tasks::Join() {
 /**
  * @brief Thread handling server communication with the monitor.
  */
-void Tasks::ServerTask(void *arg) {
-	int status;
-
+[[noreturn]] void Tasks::ServerTask(void *arg) {
 	cout << "Start " << __PRETTY_FUNCTION__ << endl;
 	// Synchronization barrier (waiting that all tasks are started)
 	rt_sem_p(&sem_barrier, TM_INFINITE);
@@ -230,19 +228,28 @@ void Tasks::ServerTask(void *arg) {
 	/* **************************************************************************************
 	 * The task server starts here
 	 * *************************************************************************************/
+
+	while (true) {
 		rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
-	status = monitor.Open(SERVER_PORT);
+		int status = monitor.Open(SERVER_PORT);
 		rt_mutex_release(&mutex_monitor);
 
-	cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl;
+		cout << "Open server on port " << SERVER_PORT << " (" << status << ")" << endl;
+
+		if (status < 0) {
+			throw runtime_error{"Unable to start server on port " + to_string(SERVER_PORT)};
+		}
 
-	if (status < 0)
-		throw runtime_error{
-				"Unable to start server on port " + to_string(SERVER_PORT)
-		};
 		monitor.AcceptClient(); // Wait the monitor client
 		cout << "Rock'n'Roll baby, client accepted!" << endl;
 		rt_sem_broadcast(&sem_serverOk);
+
+		// Wait for stopServer signal
+		rt_sem_p(&sem_stopServer, TM_INFINITE);
+		rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
+		monitor.Close();
+		rt_mutex_release(&mutex_monitor);
+	}
 }
 
 /**
@@ -283,10 +290,13 @@ void Tasks::ServerTask(void *arg) {
 	/* *************************************************************************************
 	 * The task receiveFromMon starts here
 	 * *************************************************************************************/
+
+	while (true) {
 		rt_sem_p(&sem_serverOk, TM_INFINITE);
 		cout << "Received message from monitor activated" << endl;
 
-	while (true) {
+		bool lostConnection = false;
+		while (!lostConnection) {
 			msgRcv = monitor.Read();
 			cout << "Rcv <= " << msgRcv->ToString() << endl;
 
@@ -296,6 +306,7 @@ void Tasks::ServerTask(void *arg) {
 				rt_sem_v(&sem_stopRobot);
 				rt_sem_v(&sem_stopComRobot);
 				rt_sem_v(&sem_stopServer);
+				lostConnection = true;
 			} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
 				rt_sem_v(&sem_openComRobot);
 			} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
@@ -335,6 +346,7 @@ void Tasks::ServerTask(void *arg) {
 			}
 			delete (msgRcv); // mus be deleted manually, no consumer
 		}
+	}
 }
 
 /**
@@ -471,7 +483,7 @@ void Tasks::ServerTask(void *arg) {
 			if (msgSend->CompareID(MESSAGE_ANSWER_ACK)) {
 				counter = 0;
 			} else {
-				counter ++;
+				counter++;
 			}
 			if (counter == 3) {
 				rt_sem_v(&sem_stopRobot);

software/raspberry/superviseur-robot/tasks.h

@@ -116,7 +116,7 @@ private:
     /**
      * @brief Thread handling server communication with the monitor.
      */
-    void ServerTask(void *arg);
+    [[noreturn]] void ServerTask(void *arg);
      
     /**
      * @brief Thread sending data to monitor.