Added a test for SRP Program Track (749441dd) · Commits · INAF / DISCOS

SRT/Servers/SRTMinorServo/include/SRTMinorServoSocket.h

+17 −7

Original line number	Diff line number	Diff line
		@@ -2,7 +2,7 @@
		#define __SRTMINORSERVOSOCKET_H__

		/**
		* SRTMinorServoSocketLibrary.h
		* SRTMinorServoSocket.h
		* 2023/02/23
		* Giuseppe Carboni (giuseppe.carboni@inaf.it)
		*/
		@@ -16,23 +16,22 @@

		using namespace IRA;

		// Definition of test class. It is re-defined in the unittest.cpp file.
		class SRTMinorServoSocketTest;

		class SRTMinorServoSocket: public IRA::CSocket
		{
		// Declare the SRTMinorServoSocketTest class as friend in order for it to have access to destroyInstance for testing purposes
		friend class SRTMinorServoSocketTest;
		friend class SRPProgramTrackTest;

		public:
		/**
		* Calls the constructor and returns the singleton socket instance
		* @param ip_address the IP address to which the socket will connect
		* @param port the port to which the socket will connect
		* @param timeout the timeout, in seconds, for the communication to be considered failed
		* @return the singleton socket instance, connected to the given IP address and port, by reference
		* @throw ComponentErrors::SocketErrorExImpl when the user calls this method a second time with different IP address and port arguments
		*/
		static SRTMinorServoSocket& getInstance(std::string ip_address, int port);
		static SRTMinorServoSocket& getInstance(std::string ip_address, int port, double timeout=TIMEOUT);

		/**
		* Returns the previously generated singleton socket instance
		@@ -46,7 +45,7 @@ public:
		* @param command the command to be sent over the socket
		* @return the received answer to the given command
		*/
		std::string sendCommand(std::string command);
		SRTMinorServoAnswerMap sendCommand(std::string command);

		/**
		* Copy constructor operator disabled by default
		@@ -69,8 +68,9 @@ private:
		* Constructor method. Generates the singleton socket instance
		* @param ip_address the IP address to which the socket will connect
		* @param port the port to which the socket will connect
		* @param timeout the timeout, in seconds, for the communication to be considered failed
		*/
		SRTMinorServoSocket(std::string ip_address, int port);
		SRTMinorServoSocket(std::string ip_address, int port, double timeout);

		/*
		* Destructor method. Closes the socket upon destruction
		@@ -88,11 +88,21 @@ private:
		std::string m_address;
		int m_port;

		/*
		* Timeout for communication operations
		*/
		double m_timeout;

		/*
		* Mutex object used to syncronize communications and prevent collisions between multiple threads
		*/
		std::mutex m_mutex;

		/*
		* Library mutex, used only to synchronize the getInstance methods
		*/
		static std::mutex c_mutex;

		/*
		* Socket status enumerator
		*/

SRT/Servers/SRTMinorServo/src/SRTMinorServoSocket.cpp

+14 −7

Original line number	Diff line number	Diff line
		#include "SRTMinorServoSocket.h"

		SRTMinorServoSocket& SRTMinorServoSocket::getInstance(std::string ip_address, int port)
		std::mutex SRTMinorServoSocket::c_mutex;

		SRTMinorServoSocket& SRTMinorServoSocket::getInstance(std::string ip_address, int port, double timeout)
		{
		std::lock_guard<std::mutex> guard(SRTMinorServoSocket::c_mutex);

		if(m_instance != nullptr)
		{
		if(m_instance->m_address != ip_address && m_instance->m_port != port)
		@@ -13,13 +17,15 @@ SRTMinorServoSocket& SRTMinorServoSocket::getInstance(std::string ip_address, in
		}
		else
		{
		m_instance = new SRTMinorServoSocket(ip_address, port);
		m_instance = new SRTMinorServoSocket(ip_address, port, timeout);
		}
		return *m_instance;
		}

		SRTMinorServoSocket& SRTMinorServoSocket::getInstance()
		{
		std::lock_guard<std::mutex> guard(SRTMinorServoSocket::c_mutex);

		if(m_instance == nullptr)
		{
		ComponentErrors::SocketErrorExImpl exImpl(__FILE__, __LINE__, "SRTMinorServoSocket::getInstance()");
		@@ -38,7 +44,7 @@ void SRTMinorServoSocket::destroyInstance()
		}
		}

		SRTMinorServoSocket::SRTMinorServoSocket(std::string ip_address, int port)
		SRTMinorServoSocket::SRTMinorServoSocket(std::string ip_address, int port, double timeout)
		{
		if(Create(m_error, STREAM) == FAIL)
		{
		@@ -68,6 +74,7 @@ SRTMinorServoSocket::SRTMinorServoSocket(std::string ip_address, int port)

		m_address = ip_address;
		m_port = port;
		m_timeout = timeout;
		}

		SRTMinorServoSocket::~SRTMinorServoSocket()
		@@ -76,7 +83,7 @@ SRTMinorServoSocket::~SRTMinorServoSocket()
		Close(m_error);
		}

		std::string SRTMinorServoSocket::sendCommand(std::string command)
		SRTMinorServoAnswerMap SRTMinorServoSocket::sendCommand(std::string command)
		{
		std::lock_guard<std::mutex> guard(m_mutex);

		@@ -93,7 +100,7 @@ std::string SRTMinorServoSocket::sendCommand(std::string command)
		// Reset the timer
		start_time = CIRATools::getUNIXEpoch();
		}
		else if(CIRATools::getUNIXEpoch() - start_time >= TIMEOUT)
		else if(CIRATools::getUNIXEpoch() - start_time >= m_timeout)
		{
		m_socket_status = TOUT;
		Close(m_error);
		@@ -116,7 +123,7 @@ std::string SRTMinorServoSocket::sendCommand(std::string command)
		// Reset the timer
		start_time = CIRATools::getUNIXEpoch();
		}
		else if(CIRATools::getUNIXEpoch() - start_time >= TIMEOUT)
		else if(CIRATools::getUNIXEpoch() - start_time >= m_timeout)
		{
		m_socket_status = TOUT;
		Close(m_error);
		@@ -126,5 +133,5 @@ std::string SRTMinorServoSocket::sendCommand(std::string command)
		}
		}

		return answer;
		return SRTMinorServoCommandLibrary::parseAnswer(answer);
		}

SRT/Servers/SRTMinorServo/test/Makefile

+13 −6

Original line number	Diff line number	Diff line
		@@ -14,11 +14,17 @@ USER_INC=-I$(GTEST_HOME) -I$(GMOCK_HOME)
		# unittest_OBJECTS = unittest
		# unittest_LIBS = $(GTEST_LIBS) <ComponentNameImpl>

		EXECUTABLES_L = unittest
		unittest_OBJECTS = unittest
		unittest_CFLAGS = -std=c++17
		unittest_LIBS = $(GTEST_LIBS) SRTMinorServoSocket SRTMinorServoCommandLibrary
		unittest_LDFLAGS = -lstdc++ -lpthread
		EXECUTABLES_L = SRTMinorServoSocketTest SRPProgramTrackTest

		SRTMinorServoSocketTest_OBJECTS = SRTMinorServoSocketTest
		SRTMinorServoSocketTest_CFLAGS = -std=c++17
		SRTMinorServoSocketTest_LIBS = $(GTEST_LIBS) SRTMinorServoSocket SRTMinorServoCommandLibrary IRALibrary ComponentErrors
		SRTMinorServoSocketTest_LDFLAGS = -lstdc++ -lpthread

		SRPProgramTrackTest_OBJECTS = SRPProgramTrackTest
		SRPProgramTrackTest_CFLAGS = -std=c++17
		SRPProgramTrackTest_LIBS = $(GTEST_LIBS) SRTMinorServoSocket SRTMinorServoCommandLibrary IRALibrary ComponentErrors
		SRPProgramTrackTest_LDFLAGS = -lstdc++ -lpthread

		# END OF CUSTOMIZATION
		# do not edit below this line
		@@ -40,7 +46,8 @@ endif

		do_unit: all
		@echo "running cpp unit tests"
		../bin/unittest --gtest_output=xml:results/cppunittest.xml
		../bin/SRTMinorServoSocketTest --gtest_output=xml:results/cppSRTMinorServoSocketTest.xml
		../bin/SRPProgramTrackTest --gtest_output=xml:results/cppSRPProgramTrackTest.xml

		do_pyunit:
		@echo "running python unit tests"

SRT/Servers/SRTMinorServo/test/SRPProgramTrackTest.cpp

0 → 100644

+213 −0

Original line number	Diff line number	Diff line
		#include "gtest/gtest.h"
		#include <iostream>
		#include <thread>
		#include <chrono>
		#include "SRTMinorServoSocket.h"
		#include "SRTMinorServoCommandLibrary.h"

		// This address and port are the ones set in the simulator
		// In order for the test to properly be executed, the simulator should be launched with the following command:
		// discos-simulator -s minor_servo start &
		#define ADDRESS std::string("127.0.0.1")
		#define PORT 12800
		#define TIMEOUT 0.1
		#define NOISE_THRESHOLD 0.2
		#define TIMEGAP 0.2
		#define ADVANCE_TIMEGAP 5


		std::atomic<bool> terminate = false;

		void sigintHandler(int sig_num)
		{
		std::cout << std::endl << "Terminating..." << std::endl;
		terminate = true;
		}

		std::string serializeStatus(SRTMinorServoAnswerMap map)
		{
		std::stringstream stream;
		stream << std::fixed << std::setprecision(6);
		stream << std::get<double>(map["TIMESTAMP"]);
		stream << "\t" << std::get<double>(map["SRP_TX"]);
		stream << "\t" << std::get<double>(map["SRP_TY"]);
		stream << "\t" << std::get<double>(map["SRP_TZ"]);
		stream << "\t" << std::get<double>(map["SRP_RX"]);
		stream << "\t" << std::get<double>(map["SRP_RY"]);
		stream << "\t" << std::get<double>(map["SRP_RZ"]);
		return stream.str();
		}

		std::string serializeProgramTrack(double timestamp, std::vector<double> coordinates)
		{
		std::stringstream stream;
		stream << std::fixed << std::setprecision(6) << timestamp;
		for(double coordinate : coordinates)
		{
		stream << "\t" << coordinate;
		}
		return stream.str();
		}

		class SRPProgramTrackTest : public ::testing::Test
		{
		protected:
		static std::vector<double> getCoordinates(SRTMinorServoAnswerMap SRPStatus)
		{
		std::vector<double> currentCoordinates;
		currentCoordinates.push_back(std::get<double>(SRPStatus["SRP_TX"]));
		currentCoordinates.push_back(std::get<double>(SRPStatus["SRP_TY"]));
		currentCoordinates.push_back(std::get<double>(SRPStatus["SRP_TZ"]));
		currentCoordinates.push_back(std::get<double>(SRPStatus["SRP_RX"]));
		currentCoordinates.push_back(std::get<double>(SRPStatus["SRP_RY"]));
		currentCoordinates.push_back(std::get<double>(SRPStatus["SRP_RZ"]));
		return currentCoordinates;
		}

		static bool compareCoordinates(std::vector<double> first, std::vector<double> second)
		{
		if(first.size() != second.size())
		{
		return false;
		}

		for(size_t i = 0; i < first.size(); i++)
		{
		if(first[i] != second[i])
		{
		return false;
		}
		}

		return true;
		}

		static void addNoiseToCoordinates(std::vector<double> &coordinates)
		{
		for(size_t i = 0; i < coordinates.size(); i++)
		{
		coordinates[i] += std::max(-1, std::min(1, rand() % 5 - 2)) * (double(rand() % 100) / 100) * NOISE_THRESHOLD;
		}
		}

		void SetUp() override
		{
		signal(SIGINT, sigintHandler);
		srand((int)CIRATools::getUNIXEpoch());
		std::cout << std::fixed << std::setprecision(6);

		try
		{
		SRTMinorServoSocket::getInstance(ADDRESS, PORT, TIMEOUT);
		}
		catch(ComponentErrors::SocketErrorExImpl& ex)
		{
		if(ex.getData("Reason") == std::string("Cannot connect the socket.").c_str())
		{
		FAIL() << "Socket failed to connect. Check if the simulator or the hardware can be reached." << std::endl;
		}
		else
		{
		FAIL() << "Unexpected failure." << std::endl;
		}
		}
		}

		void TearDown() override
		{
		SRTMinorServoSocket::destroyInstance();
		}
		};

		TEST_F(SRPProgramTrackTest, z_axis_translation)
		{
		SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance();
		SRTMinorServoAnswerMap::iterator iterator;

		SRTMinorServoAnswerMap MSStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status());
		EXPECT_EQ(std::get<std::string>(MSStatus["OUTPUT"]), "GOOD");
		EXPECT_EQ(std::get<int>(MSStatus["CONTROL"]), 1);
		EXPECT_EQ(std::get<int>(MSStatus["POWER"]), 1);
		EXPECT_EQ(std::get<int>(MSStatus["EMERGENCY"]), 2);
		EXPECT_EQ(std::get<int>(MSStatus["ENABLED"]), 1);

		SRTMinorServoAnswerMap SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP"));
		EXPECT_EQ(std::get<std::string>(SRPStatus["OUTPUT"]), "GOOD");
		EXPECT_EQ(std::get<int>(SRPStatus["SRP_ENABLED"]), 1);
		EXPECT_EQ(std::get<int>(SRPStatus["SRP_STATUS"]), 1);
		EXPECT_EQ(std::get<int>(SRPStatus["SRP_BLOCK"]), 2);

		/*for(iterator = SRPStatus.begin(); iterator != SRPStatus.end(); ++iterator)
		{
		std::visit([iterator](const auto& var) mutable { std::cout << iterator->first << ": " << var << std::endl; }, iterator->second);
		}*/

		std::vector<double> startingCoordinates = {0, 0, 0, 0, 0, 0};
		SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::preset("SRP", startingCoordinates));
		EXPECT_EQ(std::get<std::string>(SRPStatus["OUTPUT"]), "GOOD");

		do
		{
		std::this_thread::sleep_for(std::chrono::milliseconds(10));

		SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP"));
		EXPECT_EQ(std::get<std::string>(SRPStatus["OUTPUT"]), "GOOD");
		EXPECT_EQ(std::get<int>(SRPStatus["SRP_OPERATIVE_MODE"]), 40);
		}
		while(!compareCoordinates(startingCoordinates, getCoordinates(SRPStatus)));

		std::thread t([]()
		{
		SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance();
		SRTMinorServoAnswerMap SRPStatus;

		ofstream statusFile;
		statusFile.open("SRPStatus.txt", ios::out);

		while(!terminate)
		{
		SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP"));
		statusFile << serializeStatus(SRPStatus) << std::endl;
		std::this_thread::sleep_for(std::chrono::milliseconds(10));
		}

		statusFile.close();
		});

		double start_time = CIRATools::getUNIXEpoch() + ADVANCE_TIMEGAP;
		long unsigned int trajectory_id = int(start_time);
		unsigned int point_id = 0;
		std::vector<double> programTrackCoordinates = startingCoordinates;

		SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("SRP", trajectory_id, point_id, programTrackCoordinates, start_time));
		EXPECT_EQ(std::get<std::string>(SRPStatus["OUTPUT"]), "GOOD");

		std::this_thread::sleep_for(std::chrono::milliseconds(20));

		SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP"));
		EXPECT_EQ(std::get<std::string>(SRPStatus["OUTPUT"]), "GOOD");
		EXPECT_EQ(std::get<int>(SRPStatus["SRP_OPERATIVE_MODE"]), 50);

		ofstream programTrackFile;
		programTrackFile.open("SRPTrajectory.txt", ios::out);
		programTrackFile << serializeProgramTrack(start_time, programTrackCoordinates) << std::endl;

		double next_expected_time = start_time;

		while(!terminate)
		{
		next_expected_time += TIMEGAP;
		std::cout << next_expected_time << std::endl;

		std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, next_expected_time - ADVANCE_TIMEGAP - CIRATools::getUNIXEpoch()))));
		point_id++;

		addNoiseToCoordinates(programTrackCoordinates);
		SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("SRP", trajectory_id, point_id, programTrackCoordinates));
		EXPECT_EQ(std::get<std::string>(SRPStatus["OUTPUT"]), "GOOD");
		programTrackFile << serializeProgramTrack(next_expected_time, programTrackCoordinates) << std::endl;
		}

		programTrackFile.close();
		t.join();
		}

SRT/Servers/SRTMinorServo/test/unittest.cpp→SRT/Servers/SRTMinorServo/test/SRTMinorServoSocketTest.cpp

+187 −0

Original line number	Diff line number	Diff line
		@@ -13,6 +13,7 @@
		#define ADDRESS std::string("127.0.0.1")
		#define PORT 12800


		class SRTMinorServoSocketTest : public ::testing::Test
		{
		protected:
		@@ -37,17 +38,14 @@ protected:

		void TearDown() override
		{
		std::for_each(this->threads.begin(), this->threads.end(), [](std::thread &t)
		{
		t.join();
		});

		SRTMinorServoSocket::destroyInstance();
		}
		};

		// This test passes the already created instance to some threads
		TEST_F(SRTMinorServoSocketTest, instance_passed_to_threads)
		{
		try
		{
		SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance(ADDRESS, PORT);

		@@ -55,46 +53,101 @@ TEST_F(SRTMinorServoSocketTest, instance_passed_to_threads)
		{
		this->threads.push_back(std::thread([command, &socket]()
		{
		auto args = SRTMinorServoCommandLibrary::parseAnswer(socket.sendCommand(command));
		auto args = socket.sendCommand(command);
		// By testing if the command was received correctly we also test if the socket is working properly
		// and if the answer was received correctly without being interleaved with the answer from another thread
		EXPECT_EQ(std::get<std::string>(args["OUTPUT"]), "GOOD");
		}));
		}
		}
		catch(ComponentErrors::SocketErrorExImpl& ex)
		{
		if(ex.getData("Reason") == std::string("Cannot connect the socket.").c_str())
		{
		FAIL() << "Socket failed to connect. Check if the simulator or the hardware can be reached." << std::endl;
		}
		else
		{
		FAIL() << "Unexpected failure." << std::endl;
		}
		}

		std::for_each(this->threads.begin(), this->threads.end(), [](std::thread &t)
		{
		t.join();
		});
		}

		// This test spawns some threads, each one retrieves the instance. The first thread which tries to retrieve the instance will also generate it
		TEST_F(SRTMinorServoSocketTest, instance_retrieved_in_threads)
		{
		std::string error = "";

		for(auto command : this->commands)
		{
		this->threads.push_back(std::thread([command]()
		std::mutex mutex;

		this->threads.push_back(std::thread([command, &error, &mutex]()
		{
		try
		{
		auto args = SRTMinorServoCommandLibrary::parseAnswer(SRTMinorServoSocket::getInstance(ADDRESS, PORT).sendCommand(command));
		auto args = SRTMinorServoSocket::getInstance(ADDRESS, PORT).sendCommand(command);
		// By testing if the command was received correctly we also test if the socket is working properly
		// and if the answer was received correctly without being interleaved with the answer from another thread
		EXPECT_EQ(std::get<std::string>(args["OUTPUT"]), "GOOD");
		}
		catch(ComponentErrors::SocketErrorExImpl& ex)
		{
		std::lock_guard<std::mutex> guard(mutex);

		if(ex.getData("Reason") == std::string("Cannot connect the socket.").c_str())
		{
		error = "Socket failed to connect. Check if the simulator or the hardware can be reached.";
		}
		else
		{
		error = "Unexpected failure.";
		}
		}
		}));
		}

		std::for_each(this->threads.begin(), this->threads.end(), [](std::thread &t)
		{
		t.join();
		});

		if(error != "")
		{
		FAIL() << error << std::endl;
		}
		}

		// This test generates an instance on the given address and port, then tries to generate another instance with different address and port and fails
		TEST_F(SRTMinorServoSocketTest, open_with_args_retrieve_without)
		{
		try
		{
		// First let's open the socket with the chosen ADDRESS and PORT
		SRTMinorServoSocket::getInstance(ADDRESS, PORT);

		try
		{
		// Let's try to instance another socket on a different port
		SRTMinorServoSocket::getInstance(ADDRESS, PORT + 1);
		}
		catch(ComponentErrors::SocketErrorExImpl& ex)
		{
		if(ex.getData("Reason") == std::string("Cannot connect the socket.").c_str())
		{
		FAIL() << "Socket failed to connect. Check if the simulator or the hardware can be reached." << std::endl;
		return;
		}
		else
		{
		// Check if we got the correct exception
		EXPECT_EQ(ex.getData("Reason"), std::string("Socket already open on '" + ADDRESS + ":" + std::to_string(PORT) + "' . Use getInstance() (no arguments) to retrieve the object.").c_str());
		}
		}
		}

		// This test tries to retrieve an instance which has not been generated yet, failing
		TEST_F(SRTMinorServoSocketTest, try_open_without_args)
		@@ -104,11 +157,19 @@ TEST_F(SRTMinorServoSocketTest, try_open_without_args)
		SRTMinorServoSocket::getInstance();
		}
		catch(ComponentErrors::SocketErrorExImpl& ex)
		{
		if(ex.getData("Reason") == std::string("Cannot connect the socket.").c_str())
		{
		FAIL() << "Socket failed to connect. Check if the simulator or the hardware can be reached." << std::endl;
		return;
		}
		else
		{
		// Check if we got the correct exception
		EXPECT_EQ(ex.getData("Reason"), std::string("Socket not yet initialized. Use getInstance(std::string ip_address, int port) to initialize it and retrieve the object.").c_str());
		}
		}
		}

		// This test tries to generate an instance using a pair of address and port on which the socket fails to open
		TEST_F(SRTMinorServoSocketTest, try_open_on_wrong_address)