RI-SE · LukasWikander · Apr 14, 2020 · Jan 7, 2020 · Jan 7, 2020 · Jan 7, 2020
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -23,6 +23,7 @@ add_subdirectory(core)
 add_subdirectory(modules/ScenarioControl)
 add_subdirectory(modules/Supervision)
 add_subdirectory(modules/Visualization)
+#add_subdirectory(modules/ObjectMonitoring)
 
 # Ensure .maestro directory is created at build time in build directory
 add_custom_target(configurationDirectory ALL

diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt
@@ -103,6 +103,7 @@ target_link_libraries(${COREUTILS_TARGET} LINK_PUBLIC
 	${LOGGING_LIBRARY}
 	${MESSAGE_BUS_LIBRARY}
 	${MATH_LIBRARY}
+	${POSITIONING_TARGET}
 )
 
 target_link_libraries(${DATA_DICTIONARY_TARGET} LINK_PUBLIC

diff --git a/modules/ObjectMonitoring/CMakeLists.txt b/modules/ObjectMonitoring/CMakeLists.txt
@@ -0,0 +1,53 @@
+cmake_minimum_required(VERSION 3.1)
+SET(CMAKE_CXX_STANDARD 11)
+SET(CMAKE_CXX_STANDARD_REQUIRED ON)
+SET(CMAKE_C_STANDARD 11)
+SET(CMAKE_C_STANDARD_REQUIRED ON)
+
+project(ObjectMonitoring LANGUAGES C CXX)
+
+# Define target names
+set(OBJECT_MONITORING_TARGET ${PROJECT_NAME})
+
+set(COREUTILS_LIBRARY MaestroCoreUtil)
+set(ISO_22133_LIBRARY MaestroISO22133)
+set(POSITIONING_LIBRARY MaestroPositioning)
+set(TIME_LIBRARY MaestroTime)
+set(LOGGING_LIBRARY MaestroLogging)
+set(MESSAGE_BUS_LIBRARY MaestroMQ)
+
+include(GNUInstallDirs)
+
+find_package(Threads REQUIRED)
+
+# Create project main executable target
+add_executable(${OBJECT_MONITORING_TARGET}
+	${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp
+	${CMAKE_CURRENT_SOURCE_DIR}/src/connectionhandler.cpp
+	${CMAKE_CURRENT_SOURCE_DIR}/src/protocoldata.cpp
+	${CMAKE_CURRENT_SOURCE_DIR}/src/ISO22133ProtocolData.cpp
+)
+
+# Link project executable to util libraries
+target_link_libraries(${OBJECT_MONITORING_TARGET} LINK_PUBLIC
+	${TIME_LIBRARY}
+	${LOGGING_LIBRARY}
+	${COREUTILS_LIBRARY}
+	${CMAKE_THREAD_LIBS_INIT}
+)
+
+target_include_directories(${OBJECT_MONITORING_TARGET} PUBLIC
+	${CMAKE_CURRENT_SOURCE_DIR}/inc
+)
+
+# Installation rules
+install(CODE "MESSAGE(STATUS \"Installing target ${OBJECT_MONITORING_TARGET}\")")
+install(TARGETS ${OBJECT_MONITORING_TARGET}
+	RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
+	LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+	ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+	PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
+)
+
+# Ensure linkage is reloaded after install
+install(CODE "execute_process(COMMAND ldconfig)")
diff --git a/modules/ObjectMonitoring/README.md b/modules/ObjectMonitoring/README.md
@@ -0,0 +1,18 @@
+## Object monitoring module
+TODO
+
+### Build process
+1) Ensure your util repo is up to date
+2) Navigate to this README.md file
+3) Create the build directory: ```mkdir build```
+4) Enter the build directory: ```cd build```
+5) Generate necessary cmake files: ```cmake ..```
+6) Build the module: ```make```
+
+### Run the module
+1) Ensure you have built the module
+2) Navigate to the build directory
+3) Run the module: ```./ObjectMonitoring```
+4) Run Maestro
+
+Note: steps 3 and 4 can be replaced with running the runServer.sh script in the top directory of this repository
diff --git a/modules/ObjectMonitoring/inc/ISO22133ProtocolData.h b/modules/ObjectMonitoring/inc/ISO22133ProtocolData.h
@@ -0,0 +1,15 @@
+#ifndef ISO22133PROTOCOLDATA_H
+#define ISO22133PROTOCOLDATA_H
+
+#include "protocoldata.h"
+
+class ISO22133ProtocolData : public ProtocolData {
+public:
+	static constexpr uint16_t TCP_PORT = 12345;
+
+	ISO22133ProtocolData() {}
+	~ISO22133ProtocolData() override;
+	ReturnCode decodeFrom(const std::vector<char>& rawData) override;
+};
+
+#endif // PROTOCOLDATA_H
diff --git a/modules/ObjectMonitoring/inc/connectionhandler.h b/modules/ObjectMonitoring/inc/connectionhandler.h
@@ -0,0 +1,30 @@
+#ifndef CONNECTIONHANDLER_H
+#define CONNECTIONHANDLER_H
+
+#include <pthread.h>
+#include "protocoldata.h"
+
+class ConnectionHandler
+{
+public:
+	ConnectionHandler(int openSocketDescriptor, ProtocolData& data);
+	ConnectionHandler(int openSocketDescriptor, ProtocolData& data, unsigned long readBufferSize);
+	~ConnectionHandler();
+
+	bool isTerminated() const { return terminated; }
+private:
+	int socketDescriptor = 0;
+	ProtocolData &data;
+	unsigned long readBufferSize;
+	pthread_t readThread;
+	bool terminated = false;
+	constexpr static unsigned long DefaultReadBufferSize = 4096;
+
+	void* threadRoutine(void*);
+	static void* routineWrapper(void* context) {
+		return static_cast<ConnectionHandler *>(context)->threadRoutine(nullptr);
+	}
+	[[ noreturn ]] void terminate(void* retval);
+};
+
+#endif // CONNECTIONHANDLER_H
diff --git a/modules/ObjectMonitoring/inc/protocoldata.h b/modules/ObjectMonitoring/inc/protocoldata.h
@@ -0,0 +1,32 @@
+#ifndef PROTOCOLDATA_H
+#define PROTOCOLDATA_H
+
+#include <pthread.h>
+#include <vector>
+#include <functional>
+#include <netinet/in.h>
+#include <unordered_map>
+#include <memory>
+
+class ProtocolData {
+public:
+	ProtocolData() = default;
+	ProtocolData(const ProtocolData&) = default;
+	ProtocolData(ProtocolData&&) = default;
+	ProtocolData& operator=(const ProtocolData&) = default;
+	ProtocolData& operator=(ProtocolData&&) = default;
+	virtual ~ProtocolData();
+
+	typedef enum {
+		DECODE_PARTIAL,
+		DECODE_SUCCESSFUL,
+		DECODE_ERROR
+	} ReturnCode;
+
+	virtual ReturnCode decodeFrom(const std::vector<char> &rawData) = 0;
+
+private:
+
+};
+
+#endif // PROTOCOLDATA_H
diff --git a/modules/ObjectMonitoring/src/ISO22133ProtocolData.cpp b/modules/ObjectMonitoring/src/ISO22133ProtocolData.cpp
@@ -0,0 +1,9 @@
+#include "ISO22133ProtocolData.h"
+
+ISO22133ProtocolData::~ISO22133ProtocolData() {
+
+}
+
+ProtocolData::ReturnCode ISO22133ProtocolData::decodeFrom(const std::vector<char> &rawData) {
+	return DECODE_ERROR;
+}
diff --git a/modules/ObjectMonitoring/src/connectionhandler.cpp b/modules/ObjectMonitoring/src/connectionhandler.cpp
@@ -0,0 +1,74 @@
+#include "connectionhandler.h"
+#include "logging.h"
+
+#include <system_error>
+#include <unistd.h>
+#include <sys/socket.h>
+#include <vector>
+
+ConnectionHandler::ConnectionHandler(int openSocketDescriptor, ProtocolData& data)
+	: ConnectionHandler::ConnectionHandler(openSocketDescriptor, data, DefaultReadBufferSize) {}
+
+ConnectionHandler::ConnectionHandler(int openSocketDescriptor, ProtocolData& data, unsigned long readBufferSize) : data(data) {
+	int returnCode;
+
+	if (openSocketDescriptor < 0) {
+		LogMessage(LOG_LEVEL_ERROR, "Attempted to create connection handler with invalid socket descriptor");
+		return;
+	}
+	socketDescriptor = openSocketDescriptor;
+
+	if (readBufferSize == 0) {
+		LogMessage(LOG_LEVEL_ERROR, "Attempted to create connection handler with zero buffer size");
+		return;
+	}
+	this->readBufferSize = readBufferSize;
+
+	LogMessage(LOG_LEVEL_INFO, "Creating thread to handle connection");
+	returnCode = pthread_create(&readThread, nullptr, &ConnectionHandler::routineWrapper, this);
+	if (returnCode) {
+		LogMessage(LOG_LEVEL_ERROR, "Error creating thread");
+		close(socketDescriptor);
+		terminated = true;
+		throw std::system_error();
+	}
+}
+
+ConnectionHandler::~ConnectionHandler() {
+	close(socketDescriptor);
+}
+
+
+void* ConnectionHandler::threadRoutine(void*) {
+	std::vector<char> readBuffer(readBufferSize);
+	std::vector<char> messageBuffer;
+	ssize_t readBytes;
+
+	while ( (readBytes = read(socketDescriptor, readBuffer.data(), readBuffer.size()) ) > 0) {
+		messageBuffer.insert(messageBuffer.end(), readBuffer.begin(), readBuffer.end());
+		switch (data.decodeFrom(messageBuffer)) {
+		case ProtocolData::DECODE_PARTIAL:
+			break;
+		case ProtocolData::DECODE_SUCCESSFUL:
+			messageBuffer.clear();
+			break;
+		case ProtocolData::DECODE_ERROR:
+			messageBuffer.clear();
+			break;
+		}
+	}
+
+	if (readBytes < 0) {
+		LogMessage(LOG_LEVEL_ERROR, "Socket read error");
+		terminate(static_cast<void*>(&readBytes));
+	}
+	else {
+		LogMessage(LOG_LEVEL_INFO, "Remote closed connection");
+		terminate(nullptr);
+	}
+}
+
+void ConnectionHandler::terminate(void *retval) {
+	terminated = true;
+	pthread_exit(retval);
+}
diff --git a/modules/ObjectMonitoring/src/main.cpp b/modules/ObjectMonitoring/src/main.cpp
@@ -0,0 +1,81 @@
+#include <iostream>
+#include <unistd.h>
+#include <typeindex>
+#include <vector>
+#include <algorithm>
+
+#include "logging.h"
+#include "connectionhandler.h"
+#include "protocoldata.h"
+#include "ISO22133ProtocolData.h"
+#include "util.h"
+
+#define MODULE_NAME "ObjectMonitoring"
+#define ISO22133_PORT 53240
+
+using namespace std;
+
+static void handleNewConnection(int socketDescriptor, vector<ConnectionHandler*> &handlers);
+static void pruneTerminatedConnectionHandlers(vector<ConnectionHandler*> &handlers);
+static void listenForNewConnection(void);
+
+int main()
+{
+    COMMAND command = COMM_INV;
+    char mqRecvData[MQ_MSG_SIZE];
+    const struct timespec sleepTimePeriod = {0,10000000};
+    const struct timespec abortWaitTime = {1,0};
+	struct timespec remTime;
+	vector<ConnectionHandler*> handlers;
+
+	LogInit(MODULE_NAME, LOG_LEVEL_DEBUG);
+	LogMessage(LOG_LEVEL_INFO, "Task running with PID: %u", getpid());
+
+    // Initialize message bus connection
+	while(iCommInit()) {
+        nanosleep(&sleepTimePeriod,&remTime);
+    }
+
+	while(true) {
+		listenForNewConnection();
+
+		handleNewConnection(0, handlers);
+
+		pruneTerminatedConnectionHandlers(handlers);
+    }
+
+    return 0;
+}
+
+void handleNewConnection(int socketDescriptor, vector<ConnectionHandler*> &handlers) {
+	struct sockaddr_in socketAddress;
+	socklen_t addressLength = sizeof (socketAddress);
+	getsockname(socketDescriptor, (struct sockaddr *)&socketAddress, &addressLength);
+
+	switch(socketAddress.sin_port) {
+	case ISO22133ProtocolData::TCP_PORT:
+	{
+		ISO22133ProtocolData protoData;
+		handlers.push_back(new ConnectionHandler(socketDescriptor, protoData));
+		break;
+	}
+	default:
+		LogMessage(LOG_LEVEL_WARNING, "New connection made but no protocol specified for connected port: closing connection");
+		close(socketDescriptor);
+		return;
+	}
+}
+
+void pruneTerminatedConnectionHandlers(vector<ConnectionHandler*> &handlers) {
+	// Remove any connection handlers which are null pointers
+	handlers.erase( std::remove(handlers.begin(), handlers.end(), nullptr), handlers.end() );
+	// Remove any connection handlers which have finished their tasks
+	handlers.erase( std::remove_if(handlers.begin(), handlers.end(),
+								   [](const ConnectionHandler* handler) { return handler->isTerminated(); }
+									), handlers.end() );
+}
+
+
+void listenForNewConnection(void) {
+	// TODO
+}
diff --git a/modules/ObjectMonitoring/src/protocoldata.cpp b/modules/ObjectMonitoring/src/protocoldata.cpp
@@ -0,0 +1,4 @@
+#include "protocoldata.h"
+
+ProtocolData::~ProtocolData() {
+}