RI-SE · LukasWikander · Jan 12, 2021 · Dec 11, 2020 · Dec 15, 2020 · Dec 15, 2020
diff --git a/core/inc/util.h b/core/inc/util.h
@@ -684,7 +684,7 @@ int UtilFindCurrentTrajectoryPositionNew(ObjectPosition *OP, int StartIndex, dou
 int UtilSetSyncPoint(ObjectPosition *OP, double x, double y, double z, double time);
 float UtilCalculateTimeToSync(ObjectPosition *OP);
 
-char UtilIsPointInPolygon(CartesianPosition point, CartesianPosition *polygonPoints, unsigned int nPtsInPolygon);
+char UtilIsPointInPolygon(const CartesianPosition point, const CartesianPosition* polygonPoints, unsigned int nPtsInPolygon);
 
 int UtilCountFileRows(FILE *fd);
 int UtilReadLineCntSpecChars(FILE *fd, char *Buffer);

diff --git a/...s/DONTRUNYET_905-geofenceTransgression.py → ...ration-tests/905-geofenceTransgression.py b/...s/DONTRUNYET_905-geofenceTransgression.py → ...ration-tests/905-geofenceTransgression.py
@@ -76,34 +76,39 @@ def geofenceTransgressionTest():
     # Start
     mscp.Start(0)
     mscp.waitForObjectControlState("RUNNING",timeout=0.5)
+    time.sleep(0.01) # Await one new HEAB
+    assert obj.lastCCStatus() == "running", "HEAB state not set to running after start"
 
     # Report a number of MONR inside geofence
     print("=== Entered running state, sending test MONR data")
     obj.MONR(transmitter_id=objID,position=testPts[0])
-    sleep(0.001*random.randint(1,7))
+    time.sleep(0.001*random.randint(1,7))
     obj.MONR(transmitter_id=objID,position=testPts[1])
-    sleep(0.001*random.randint(1,7))
+    time.sleep(0.001*random.randint(1,7))
     obj.MONR(transmitter_id=objID,position=testPts[2])
-    sleep(0.001*random.randint(1,7))
+    time.sleep(0.001*random.randint(1,7))
     obj.MONR(transmitter_id=objID,position=testPts[3])
-    sleep(0.001*random.randint(1,7))
+    time.sleep(0.001*random.randint(1,7))
 
     # Check last HEAB so it is not ABORT
-    assert obj.lastCCStatus() == "running"
+    print(obj.lastCCStatus())
+    assert obj.lastCCStatus() == "running", "HEAB state not kept at running after valid positions"
+    print("=== Sending transgressing MONR data")
 
     # Report one MONR outside geofence
     obj.MONR(transmitter_id=objID,position=testPts[4])
     transgressionTime = time.time()
-    sleep(0.001*random.randint(1,7))
+    time.sleep(0.01)
+
     obj.MONR(transmitter_id=objID,position=testPts[5])
-    sleep(0.001*random.randint(1,7))
+    time.sleep(0.001*random.randint(1,7))
     obj.MONR(transmitter_id=objID,position=testPts[6])
 
     # Sleep until max allowed time passed
     time.sleep(maxAbortDelay-(time.time()-transgressionTime))
 
     # Check last HEAB so it is ABORT
-    assert obj.lastCCStatus() == "abort"
+    assert obj.lastCCStatus() == "abort", "HEAB state not set to abort after exiting geofence"
     return
 
 
@@ -119,4 +124,6 @@ def geofenceTransgressionTest():
             sup.stop()
         if core:
             core.stop()
+        if obj:
+            obj.shutdown()
 
diff --git a/core/integration-tests/tools/ISO.py b/core/integration-tests/tools/ISO.py
@@ -81,19 +81,19 @@ def MONR(self, transmitter_id=None, timestamp=None, in_position=None, heading_de
         try:
             position['x'] = as_hex_little_endian(in_position['x']*1000, 4)
             position['y'] = as_hex_little_endian(in_position['y']*1000, 4)
-        except KeyError:
+        except (KeyError,TypeError):
             position['x'] = as_hex_little_endian(in_position[0]*1000, 4)
             position['y'] = as_hex_little_endian(in_position[1]*1000, 4)
 
         position['z'] = as_hex_little_endian(0, 4)
         try:
             position['z'] = as_hex_little_endian(in_position['z']*1000, 4)
-        except KeyError:
+        except  (KeyError,TypeError):
             try:
                 position['z'] = as_hex_little_endian(in_position[2]*1000, 4)
             except (IndexError, KeyError):
                 pass
-        heading_deg = as_hex_little_endian(heading_deg*100, 2)
+        heading_deg = as_hex_little_endian(((90 - heading_deg + 360) % 360)*100, 2)
         speed['longitudinal'] = as_hex_little_endian(speed['longitudinal'], 2)
         speed['lateral'] = as_hex_little_endian(speed['lateral'], 2)
         acceleration['longitudinal'] = as_hex_little_endian(acceleration['longitudinal'], 2)
@@ -180,9 +180,12 @@ def create_connection(self):
             self.udpSocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
             self.udpSocket.bind((self.host, self.port))
             while not self.quit:
-                data, remoteAddr = self.udpSocket.recvfrom(2048)
-                self.remoteAddr = remoteAddr
-                self.lastHEAB = data
+                ready = select.select([self.udpSocket],[],[],0)
+                if ready[0]:
+                    data, remoteAddr = self.udpSocket.recvfrom(2048)
+                    self.remoteAddr = remoteAddr
+                    self.lastHEAB = data
+                time.sleep(0.001)
 
         def send(self,message):
             if not self.remoteAddr:

diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c
@@ -684,7 +684,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) {
 				SearchStartIndex = -1;
 				ASPData.PrevTimeToSyncPointDbl = 0;
 				OldTimeU32 = CurrentTimeU32;
-				objectControlServerStatus = CONTROL_CENTER_STATUS_READY;
 
 				for (iIndex = 0; iIndex < nbr_objects; ++iIndex) {
 					if (!hasDelayedStart
@@ -699,8 +698,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) {
 					}
 				}
 				vSetState(OBC_STATE_RUNNING, GSD);
+				objectControlServerStatus = CONTROL_CENTER_STATUS_RUNNING;
 
-				//OBCState = OBC_STATE_INITIALIZED; //This is temporary!
 				//printf("OutgoingStartTimeU32 = %d\n", OutgoingStartTimeU32);
 				GSD->ScenarioStartTimeU32 = TimeGetAsGPSqmsOfWeek(&startTime) >> 2;
 				bzero(MiscText, SMALL_BUFFER_SIZE_0);

diff --git a/core/src/util.c b/core/src/util.c
@@ -1165,7 +1165,8 @@ float UtilCalculateTimeToSync(ObjectPosition * OP) {
  * \param nPtsInPolygon length of the vertex array
  * \return true if the point lies within the polygon, false otherwise
  */
-char UtilIsPointInPolygon(CartesianPosition point, CartesianPosition * vertices, unsigned int nPtsInPolygon) {
+char UtilIsPointInPolygon(const CartesianPosition point, const CartesianPosition * vertices,
+						  unsigned int nPtsInPolygon) {
 	int nIntersects = 0;
 
 	if (nPtsInPolygon == 0) {
@@ -3774,7 +3775,7 @@ int UtilGetObjectFileSetting(const enum ObjectFileParameter setting, const char
 	memset(objectSetting, 0, objectSettingSize);
 	UtilSearchTextFile(objectFilePath, textBuffer, "", objectSetting);
 
-	LogMessage(LOG_LEVEL_DEBUG, "Read object parameter: %s%s\n", textBuffer, objectSetting);
+	LogMessage(LOG_LEVEL_DEBUG, "Read object parameter: %s%s", textBuffer, objectSetting);
 
 	return objectSetting[0] == '\0' ? -1 : 0;
 }

diff --git a/modules/Supervision/CMakeLists.txt b/modules/Supervision/CMakeLists.txt
@@ -24,6 +24,7 @@ add_executable(${SUPERVISION_TARGET}
 	${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp
 	${CMAKE_CURRENT_SOURCE_DIR}/src/supervisionstate.cpp
 	${CMAKE_CURRENT_SOURCE_DIR}/src/trajectory.cpp
+	${CMAKE_CURRENT_SOURCE_DIR}/src/objectconfiguration.cpp
 )
 
 # Link project executable to util libraries

diff --git a/modules/Supervision/inc/geofence.h b/modules/Supervision/inc/geofence.h
diff --git a/modules/Supervision/inc/geofence.hpp b/modules/Supervision/inc/geofence.hpp
@@ -0,0 +1,36 @@
+#ifndef	GEOFENCE_H
+#define	GEOFENCE_H
+
+#include <iostream>
+#include <vector>
+#include <regex>
+
+#include "util.h"
+#include "positioning.h"
+
+class Geofence
+{
+public:
+
+	Geofence() = default;
+	~Geofence() { polygonPoints.clear(); }
+	Geofence(const Geofence& other);
+
+    std::vector<CartesianPosition> polygonPoints;
+    std::string name = "";
+    bool isPermitted = false;
+    double minHeight = 0;
+    double maxHeight = 0;
+
+	bool permits(const CartesianPosition &position) const;
+	bool forbids(const CartesianPosition &position) const;
+
+	void initializeFromFile(const std::string& fileName);
+
+private:
+	static const std::regex fileHeaderPattern;
+	static const std::regex fileLinePattern;
+	static const std::regex fileFooterPattern;
+};
+
+#endif
diff --git a/modules/Supervision/inc/objectconfiguration.hpp b/modules/Supervision/inc/objectconfiguration.hpp
@@ -0,0 +1,17 @@
+#ifndef OBJECTDATA_HPP
+#define OBJECTDATA_HPP
+#include "trajectory.hpp"
+
+class ObjectConfiguration
+{
+public:
+	ObjectConfiguration();
+	ObjectConfiguration(const std::string &fromFile);
+	Trajectory trajectory;
+	in_addr_t ip = 0;
+	uint32_t id = 0;
+
+	void initializeFromFile(const std::string &fileName);
+};
+
+#endif // OBJECTDATA_HPP
diff --git a/modules/Supervision/inc/regexpatterns.h → modules/Supervision/inc/regexpatterns.hpp b/modules/Supervision/inc/regexpatterns.h → modules/Supervision/inc/regexpatterns.hpp
diff --git a/modules/Supervision/inc/supervisionstate.h → modules/Supervision/inc/supervisionstate.hpp b/modules/Supervision/inc/supervisionstate.h → modules/Supervision/inc/supervisionstate.hpp
diff --git a/modules/Supervision/inc/trajectory.h → modules/Supervision/inc/trajectory.hpp b/modules/Supervision/inc/trajectory.h → modules/Supervision/inc/trajectory.hpp
@@ -163,13 +163,12 @@ class Trajectory {
     std::vector<TrajectoryPoint> points;
     std::string name = "";
     int version = 0;
-    int id = 0;
-    in_addr_t ip = 0;
+	int id = 0;
 
-    void initializeFromFile(const std::string fileName);
+	void initializeFromFile(const std::string& fileName);
 private:
-    static const std::regex fileHeaderPattern;
-    static const std::regex fileLinePattern;
-    static const std::regex fileFooterPattern;
+	static const std::regex fileHeaderPattern;
+	static const std::regex fileLinePattern;
+	static const std::regex fileFooterPattern;
 };
 #endif
diff --git a/modules/Supervision/src/geofence.cpp b/modules/Supervision/src/geofence.cpp
@@ -1 +1,130 @@
-#include "geofence.h"
+#include <fstream>
+
+#include "geofence.hpp"
+#include "util.h"
+#include "regexpatterns.hpp"
+
+const std::regex Geofence::fileHeaderPattern("GEOFENCE;([a-zA-Z0-9]+);(" + RegexPatterns::intPattern
+					+ ");(permitted|forbidden);(" + RegexPatterns::floatPattern + ");("
+							   + RegexPatterns::floatPattern + ");");
+const std::regex Geofence::fileLinePattern("LINE;(" + RegexPatterns::floatPattern + ");("
+							 + RegexPatterns::floatPattern + ");ENDLINE;");
+const std::regex Geofence::fileFooterPattern("ENDGEOFENCE;");
+
+Geofence::Geofence(const Geofence& other) {
+	this->name = other.name;
+	this->maxHeight = other.maxHeight;
+	this->minHeight = other.minHeight;
+	this->isPermitted = other.isPermitted;
+	this->polygonPoints = std::vector<CartesianPosition>(other.polygonPoints);
+}
+
+/*!
+* \brief parseGeofenceFile Parse the given file into a Geofence object
+* \param geofenceFile A string representing the filename relative to the geofence directory.
+*/
+void Geofence::initializeFromFile(const std::string &fileName) {
+
+	using namespace std;
+	char geofenceDirPath[PATH_MAX];
+	ifstream file;
+	smatch match;
+	string line;
+	string errMsg;
+
+	bool isHeaderParsedSuccessfully = false;
+	unsigned long nPoints = 0;
+
+	UtilGetGeofenceDirectoryPath(geofenceDirPath, sizeof (geofenceDirPath));
+	string geofenceFilePath(geofenceDirPath);
+	geofenceFilePath += fileName;
+
+	file.open(geofenceFilePath);
+	if (!file.is_open()) {
+		throw ifstream::failure("Unable to open file <" + geofenceFilePath + ">");
+	}
+
+	errMsg = "Encountered unexpected end of file while reading file <" + geofenceFilePath + ">";
+
+	for (unsigned long lineCount = 0; getline(file, line); lineCount++) {
+		if (lineCount == 0) {
+			if (regex_search(line, match, fileHeaderPattern)) {
+				name = match[1];
+				nPoints = stoul(match[2]);
+				polygonPoints.reserve(nPoints);
+				isPermitted = match[3].compare("permitted") == 0;
+				minHeight = stod(match[4]);
+				minHeight = stod(match[5]);
+				isHeaderParsedSuccessfully = true;
+			}
+			else {
+				errMsg = "The header of geofence file <" + geofenceFilePath + "> is badly formatted";
+				break;
+			}
+		}
+		else if (lineCount > 0 && !isHeaderParsedSuccessfully) {
+			errMsg = "Attempt to parse geofence file <" + geofenceFilePath + "> before encountering header";
+			break;
+		}
+		else if (lineCount > nPoints + 1) {
+			errMsg = "Geofence line count of file <" + geofenceFilePath
+					+ "> does not match specified line count";
+			break;
+		}
+		else if (lineCount == nPoints + 1) {
+			if (regex_search(line, match, fileFooterPattern)) {
+				file.close();
+				LogMessage(LOG_LEVEL_DEBUG, "Closed <%s>", geofenceFilePath.c_str());
+				return;
+			}
+			else {
+				errMsg = "Final line of geofence file <" + geofenceFilePath + "> badly formatted";
+				break;
+			}
+		}
+		else {
+			if (regex_search(line, match, fileLinePattern)) {
+				CartesianPosition pos;
+				pos.xCoord_m = stod(match[1]);
+				pos.yCoord_m = stod(match[2]);
+				pos.zCoord_m = (maxHeight + minHeight) / 2.0;
+				pos.isPositionValid = true;
+				pos.heading_rad = 0;
+				pos.isHeadingValid = false;
+
+				LogMessage(LOG_LEVEL_DEBUG, "Point: (%.3f, %.3f, %.3f)",
+						   pos.xCoord_m,
+						   pos.yCoord_m,
+						   pos.zCoord_m);
+				polygonPoints.push_back(pos);
+			}
+			else {
+				errMsg = "Line " + to_string(lineCount) + " of geofence file <"
+						+ geofenceFilePath + "> badly formatted";
+				break;
+			}
+		}
+	}
+	file.close();
+	LogMessage(LOG_LEVEL_DEBUG, "Closed <%s>", geofenceFilePath.c_str());
+	LogMessage(LOG_LEVEL_ERROR, errMsg.c_str());
+	throw invalid_argument(errMsg);
+}
+
+bool Geofence::forbids(const CartesianPosition &position) const {
+	return !permits(position);
+}
+
+bool Geofence::permits(const CartesianPosition &position) const {
+	char isInPolygon = UtilIsPointInPolygon(position, polygonPoints.data(),
+											static_cast<unsigned int>(polygonPoints.size()));
+	if (isInPolygon < 0) {
+		throw std::invalid_argument("No points in geofence");
+	}
+
+	if ((isPermitted && isInPolygon)
+		|| (!isPermitted && !isInPolygon)) {
+			return true;
+	}
+	return false;
+}