From 3a6ae4db19d1118ec4f293c27316ed52395afdac Mon Sep 17 00:00:00 2001
From: Yohan Simard <ysimard@etud.insa-toulouse.fr>
Date: Mon, 15 Mar 2021 10:51:03 +0100
Subject: [PATCH] Improve receiveFromMon and serverTask

---
 .../superviseur-robot/lib/commonitor.cpp      |   5 +-
 .../superviseur-robot/lib/commonitor.h        |   2 +-
 .../raspberry/superviseur-robot/tasks.cpp     | 132 ++++++++++--------
 software/raspberry/superviseur-robot/tasks.h  |   2 +-
 4 files changed, 78 insertions(+), 63 deletions(-)

diff --git a/software/raspberry/superviseur-robot/lib/commonitor.cpp b/software/raspberry/superviseur-robot/lib/commonitor.cpp
index bb64388..c1d6fe5 100644
--- a/software/raspberry/superviseur-robot/lib/commonitor.cpp
+++ b/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;
 }
diff --git a/software/raspberry/superviseur-robot/lib/commonitor.h b/software/raspberry/superviseur-robot/lib/commonitor.h
index 172c5b2..06db8e4 100644
--- a/software/raspberry/superviseur-robot/lib/commonitor.h
+++ b/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
diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp
index 6737d17..6826c34 100644
--- a/software/raspberry/superviseur-robot/tasks.cpp
+++ b/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
 	 * *************************************************************************************/
-	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;
+	while (true) {
+		rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
+		int status = monitor.Open(SERVER_PORT);
+		rt_mutex_release(&mutex_monitor);
 
-	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);
+		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)};
+		}
+
+		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,57 +290,62 @@ void Tasks::ServerTask(void *arg) {
 	/* *************************************************************************************
 	 * The task receiveFromMon starts here
 	 * *************************************************************************************/
-	rt_sem_p(&sem_serverOk, TM_INFINITE);
-	cout << "Received message from monitor activated" << endl;
 
 	while (true) {
-		msgRcv = monitor.Read();
-		cout << "Rcv <= " << msgRcv->ToString() << endl;
+		rt_sem_p(&sem_serverOk, TM_INFINITE);
+		cout << "Received message from monitor activated" << endl;
 
-		if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
-			cout << "Monitor connection lost! Stopping robot..." << endl;
+		bool lostConnection = false;
+		while (!lostConnection) {
+			msgRcv = monitor.Read();
+			cout << "Rcv <= " << msgRcv->ToString() << endl;
+
+			if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
+				cout << "Monitor connection lost! Stopping robot..." << endl;
 //			rt_sem_v(&sem_stopCamera); // TODO
-			rt_sem_v(&sem_stopRobot);
-			rt_sem_v(&sem_stopComRobot);
-			rt_sem_v(&sem_stopServer);
-		} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
-			rt_sem_v(&sem_openComRobot);
-		} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
-			rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE);
-			watchdogMode = WITHOUT_WATCHDOG;
-			rt_mutex_release(&mutex_watchdogMode);
-			rt_sem_v(&sem_startRobot);
-		} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
-			// TODO: gérer la watchdog
-			rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE);
-			watchdogMode = WITH_WATCHDOG;
-			rt_mutex_release(&mutex_watchdogMode);
-			rt_sem_v(&sem_startRobot);
-		} 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_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)) {
+				rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE);
+				watchdogMode = WITHOUT_WATCHDOG;
+				rt_mutex_release(&mutex_watchdogMode);
+				rt_sem_v(&sem_startRobot);
+			} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
+				// TODO: gérer la watchdog
+				rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE);
+				watchdogMode = WITH_WATCHDOG;
+				rt_mutex_release(&mutex_watchdogMode);
+				rt_sem_v(&sem_startRobot);
+			} 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);
-		} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
-			// TODO
-		} else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) {
-			// TODO
-		} else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
-			// TODO
-		} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) {
-			// TODO
-		} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) {
-			// TODO
-		} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) {
-			// TODO
-		} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)) {
-			// TODO
+				rt_mutex_acquire(&mutex_move, TM_INFINITE);
+				move = msgRcv->GetID();
+				rt_mutex_release(&mutex_move);
+			} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
+				// TODO
+			} else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) {
+				// TODO
+			} else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
+				// TODO
+			} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) {
+				// TODO
+			} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) {
+				// TODO
+			} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) {
+				// TODO
+			} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)) {
+				// TODO
+			}
+			delete (msgRcv); // mus be deleted manually, no consumer
 		}
-		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);
diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h
index 1ef9457..886f451 100644
--- a/software/raspberry/superviseur-robot/tasks.h
+++ b/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.