Async and receiver transmitter

.gitmodules

@@ -3,4 +3,4 @@
 	url = https://github.com/RI-SE/iso22133
 [submodule "sigslot"]
 	path = sigslot
-	url = git@github.com:palacaze/sigslot.git
+	url = https://github.com/palacaze/sigslot.git

.vscode/settings.json

@@ -0,0 +1,100 @@
+{
+    "files.associations": {
+        "*.ipp": "cpp",
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "csetjmp": "cpp",
+        "csignal": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "any": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "bit": "cpp",
+        "*.tcc": "cpp",
+        "bitset": "cpp",
+        "chrono": "cpp",
+        "compare": "cpp",
+        "concepts": "cpp",
+        "condition_variable": "cpp",
+        "coroutine": "cpp",
+        "cstdint": "cpp",
+        "deque": "cpp",
+        "forward_list": "cpp",
+        "list": "cpp",
+        "map": "cpp",
+        "set": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "string": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "fstream": "cpp",
+        "initializer_list": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "numbers": "cpp",
+        "ostream": "cpp",
+        "ranges": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "stop_token": "cpp",
+        "streambuf": "cpp",
+        "thread": "cpp",
+        "cinttypes": "cpp",
+        "typeinfo": "cpp",
+        "semaphore": "cpp",
+        "codecvt": "cpp",
+        "__nullptr": "cpp",
+        "future": "cpp",
+        "cerrno": "cpp",
+        "__split_buffer": "cpp",
+        "regex": "cpp",
+        "__hash_table": "cpp",
+        "__tree": "cpp",
+        "queue": "cpp",
+        "stack": "cpp",
+        "variant": "cpp",
+        "filesystem": "cpp",
+        "shared_mutex": "cpp",
+        "__mutex_base": "cpp",
+        "__functional_base": "cpp",
+        "locale": "cpp",
+        "strstream": "cpp",
+        "header.h": "c",
+        "iso22133.h": "c",
+        "__locale": "cpp",
+        "__string": "cpp",
+        "valarray": "cpp",
+        "__node_handle": "cpp",
+        "source_location": "cpp",
+        "typeindex": "cpp",
+        "complex": "cpp",
+        "iomanip": "cpp"
+    }
+}

CMakeLists.txt

@@ -19,10 +19,17 @@ set(WITH_SWIG OFF CACHE BOOL "Enable swigging")
 set(SWIG_WITH_JAVA OFF CACHE BOOL "Swig to target-language java")
 set(SWIG_WITH_PYTHON OFF CACHE BOOL "Swig to target-language python")
 
+if (BUILD_TESTING)
+    set(WITH_SWIG ON)
+    set(SWIG_WITH_PYTHON ON)
+endif()
+
 if(SWIG_WITH_JAVA)
     set(SWIG_TARGET_LANG java)
+    add_compile_definitions(WITH_SWIG)
 elseif(SWIG_WITH_PYTHON)
     set(SWIG_TARGET_LANG python)
+    add_compile_definitions(WITH_SWIG)
 endif()
 
 # Manage namespace of the project
@@ -211,4 +218,4 @@ if (BUILD_TESTING)
 	)
 	include(GoogleTest)
 	gtest_discover_tests(${ISOOBJECT_LIBRARY}_test)
-endif()
+endif()

inc/iso22133object.hpp

@@ -5,7 +5,6 @@
 #include <thread> 
 #include <mutex>
 #include <atomic>
-#include <optional>
 
 #include "header.h"
 #include "iso22133.h"
@@ -29,6 +28,8 @@ class Aborting;
 class PreArming;
 class PreRunning;
 
+
+#define DEFAULT_CONTROL_CENTER_ID 0 // 0 is reserved for the control center
 /**
  * @brief The TestObject class is an abstract form of  a
  *          ISO22133 object. It needs to be ineherited and
@@ -51,9 +52,11 @@ class TestObject {
 
 public:
 	TestObject(const std::string& listenIP = "0.0.0.0");
+	TestObject(int tcpSocketNativeHandle);
 	virtual ~TestObject();
 
 	void disconnect();
+	void shutdown();
 
 	void setPosition(const CartesianPosition& pos) { position = pos; }
 	void setSpeed(const SpeedType& spd) { speed = spd; }
@@ -66,6 +69,7 @@ class TestObject {
 	void setErrorState(const char err) { errorState = err; }
 
 	std::string getCurrentStateName() const { return state->getName(); }
+	int getCurrentStateID() const { return state->getStateID(); }
 	std::string getName() const { return name; }
 	CartesianPosition getPosition() const { return position; }
 	SpeedType getSpeed() const { return speed; }
@@ -76,11 +80,29 @@ class TestObject {
 	GeographicPositionType getOrigin() const { return origin; }
 	std::string getLocalIP() const { return localIP; }
 	uint32_t getTransmitterID() const { return transmitterID; }
+	uint32_t getReceiverID() const { return receiverID; }
 	ObjectSettingsType getObjectSettings() const { return objectSettings; }
 
+#ifdef WITH_SWIG
+	/** SWIG Wrappers **/
+	//! Wrapper for handling tcp messages when using an iso connector for simulation
+	int handleTCPMessage(char *buffer, int bufferLen);
+	//! Wrapper for handling udp message when using an iso connector for simulation
+	int handleUDPMessage(char *buffer, int bufferLen, int udpSocket, char *addr, const uint32_t port);
+#endif
 
+	//! Used to start the threads
+	void startHandleTCP() { tcpReceiveThread = std::thread(&TestObject::receiveTCP, this); }
+	void startHandleUDP() { udpReceiveThread = std::thread(&TestObject::receiveUDP, this); }
+	void startHEABCheck() { heabTimeoutThread = std::thread(&TestObject::checkHeabLoop, this); }
+	void startSendMonr()  { monrThread = std::thread(&TestObject::sendMonrLoop, this); }
 
 protected:
+	//! Wrapper for handling function that converts to char vector
+	int handleMessage(char *buffer, int bufferLen);
+
+	//! Fill message header with receiver/transmitter id and messageCounter. Returns pointer to input header.
+	MessageHeaderType *populateMessageHeader(MessageHeaderType *header);
 
 	//! Pure virtual safety function that must be implemented by the user.
 	virtual void handleAbort() { throw std::logic_error("Use of unimplemented abort handler"); }
@@ -116,7 +138,6 @@ class TestObject {
 	//! Must be overridden if modifying the Pre-Running state
 	virtual PreRunning* createPreRunning() const { return new PreRunning; }
 
-
 	//! Signals for events
 	sigslot::signal<> stateChangeSig;
 	sigslot::signal<ObjectSettingsType&> osemSig;
@@ -147,6 +168,8 @@ class TestObject {
 
 	ISO22133::State* state;
 private:
+	//! Initializer for commonalities of the constructs
+	void initialize();
 
 	//! TCP receiver loop that should be run in its own thread.
 	void receiveTCP();
@@ -157,12 +180,6 @@ class TestObject {
 	//! MONR sending loop that should be run in its own thread.
 	void sendMonrLoop();
 
-	//! Used to start the threads
-	void startHandleTCP() { tcpReceiveThread = std::thread(&TestObject::receiveTCP, this); }
-	void startHandleUDP() { udpReceiveThread = std::thread(&TestObject::receiveUDP, this); }
-	void startHEABCheck() { heabTimeoutThread = std::thread(&TestObject::checkHeabLoop, this); }
-	void startSendMonr()  { monrThread = std::thread(&TestObject::sendMonrLoop, this); }
-
 	//! Function for handling received ISO messages. Calls corresponding
 	//! handler in the current state.
 	int handleMessage(std::vector<char>&);
@@ -179,11 +196,21 @@ class TestObject {
 	//! Set estimated network delay from HEAB times
 	void setNetworkDelay(std::chrono::milliseconds);
 
+	//! Get the Next message counter to send
+	char getNextSentMessageCounter() { return sentMessageCounter = (sentMessageCounter + 1) % 256; }
+
+	//! Check if the received message counter is correct and update regardless to the next expected
+	void checkAndUpdateMessageCounter(const char receivedMessageCounter) {
+		if (receivedMessageCounter != expectedMessageCounter) {
+			std::cout << "Message counter mismatch. Expected: " << (int)expectedMessageCounter << " Received: " << (int)receivedMessageCounter << std::endl;
+		}
+		expectedMessageCounter = (expectedMessageCounter + 1) % 256;
+	}
+
 	sigslot::signal<>heabTimeout;
 	std::mutex recvMutex;
 	std::mutex heabMutex;
 	std::mutex netwrkDelayMutex;
-    std::mutex disconnectMutex;
 	std::string localIP;
 	std::thread tcpReceiveThread;
 	std::thread udpReceiveThread;
@@ -206,8 +233,13 @@ class TestObject {
 	std::atomic<ObjectStateID> objectState  { ISO_OBJECT_STATE_UNKNOWN };
 	std::atomic<int> readyToArm { OBJECT_READY_TO_ARM_UNAVAILABLE };
 	std::atomic<int> transmitterID;
+	std::atomic<int> receiverID;
+	std::atomic<char> expectedMessageCounter;
+	std::atomic<char> sentMessageCounter;
+	std::atomic<bool> socketsReceivedFromController { false };
 	std::atomic<char> errorState { 0 };
 	std::atomic<bool> awaitingFirstHeab { true };
+	std::atomic<bool> osemReceived { false };
 	std::atomic<bool> on { true };
 
 	std::chrono::milliseconds estimatedNetworkDelay = std::chrono::milliseconds(0);

inc/iso22133state.hpp

@@ -7,6 +7,7 @@
 #include <functional>
 extern "C"{
 #include "iso22133.h"
+#include "header.h"
 }
 namespace ISO22133 {
 class TestObject;
@@ -149,7 +150,6 @@ class Off : public State {
 class Init : public State {
 public:
 	virtual ObjectStateID getStateID() const final override { return ISO_OBJECT_STATE_INIT; }
-	virtual void onExit(TestObject&) override;
 private:
 	void handleTRAJ(TestObject&, std::atomic<HeaderType>&) final override { unexpectedMessageWarning("TRAJ"); }
 	void handleOSEM(TestObject&, ObjectSettingsType&) final override { unexpectedMessageWarning("OSEM"); }

inc/tcpServer.hpp

@@ -14,23 +14,55 @@
  */
 class TcpServer {
    public:
-	TcpServer(std::string ip, uint32_t port) : acceptor(context, boost::asio::ip::tcp::endpoint(boost::asio::ip::address_v4::from_string(ip), port)), socket(context) {
+	TcpServer(std::string ip, uint32_t port) :
+	acceptor(context, boost::asio::ip::tcp::endpoint(boost::asio::ip::address_v4::from_string(ip), port)),
+	socket(context),
+	acceptIncoming(true) {
 		setBufferSize(defaultBufferSize);
 	};
+
+	TcpServer(int sock) : 
+	socket(context),
+	acceptor(context),
+	acceptIncoming(false) {
+		setBufferSize(defaultBufferSize);
+		socket.assign(boost::asio::ip::tcp::v4(), sock);
+	};
+
 	virtual ~TcpServer() = default;
 	void disconnect() {
 		try {
-			socket.shutdown(boost::asio::socket_base::shutdown_both);
-			socket.close();
-		} catch (boost::system::system_error& e) {}
+			if (this->acceptIncoming) {
+				acceptor.cancel();
+			}
+			if (socket.is_open()){
+				socket.shutdown(boost::asio::socket_base::shutdown_both);
+				socket.close();
+			}
+			context.reset();
+		} catch (boost::system::system_error& e) {
+			std::cerr << "Error when closing socket: " << e.what() << std::endl;
+		}
 	};
 
 	void acceptConnection() {
-		try {
-			acceptor.accept(socket);
-		} catch (boost::system::system_error& e) {
-			std::cerr << "TCP socket accept failed: " << e.what() << std::endl;
+		if (!this->acceptIncoming) {
+			throw std::runtime_error("Cannot accept connections on a socket that is not listening");
 		}
+		acceptor.async_accept(socket, [this](const boost::system::error_code& error) {
+			if (error) {
+				// Accepting failed, handle the error
+				// Print the error message for example
+				if (error == boost::asio::error::operation_aborted) {
+					std::cerr << "TCP Accept aborted" << std::endl;
+				} else {
+					std::cerr << "TCP Accept error: " << error.message() << std::endl;
+					throw boost::system::system_error(boost::asio::error::eof);
+				}
+			}
+		});
+		context.run_one();
+		context.restart();
 	}
 
 	void setBufferSize(size_t size) { dataBuffer.resize(size); };
@@ -77,6 +109,7 @@ class TcpServer {
    private:
 	std::vector<char> dataBuffer;
 	size_t defaultBufferSize = 4096;
+	bool acceptIncoming;
 
 	boost::asio::io_context context;
 	boost::asio::ip::tcp::acceptor acceptor;

inc/trajDecoder.hpp

@@ -6,6 +6,7 @@
 #include <atomic>
 
 #include "iso22133.h"
+#include "traj.h"
 
 /**
  * @brief Class for decoding TRAJ messages. Stores TRAJ data and

inc/udpServer.hpp

@@ -15,9 +15,19 @@
  */
 class UdpServer {
    public:
-	UdpServer(std::string ip, uint32_t port) : socket(context, boost::asio::ip::udp::endpoint(boost::asio::ip::address_v4::from_string(ip), port)) {
+	UdpServer(const std::string &ip, uint32_t port) : 
+	socket(context, boost::asio::ip::udp::endpoint(boost::asio::ip::address_v4::from_string(ip), port)) {
 		setBufferSize(defaultBufferSize);
 	};
+	UdpServer() :
+	socket(context) {
+		setBufferSize(defaultBufferSize);
+	};
+
+	void setEndpoint(int native_socket, boost::asio::ip::udp::endpoint &ep) {
+		socket.assign(boost::asio::ip::udp::v4(), native_socket);
+		senderEndpoint = ep;
+	};
 
 	void setBufferSize(size_t size) { dataBuffer.resize(size); };