Commit 4bfce059 authored by Giuseppe Carboni's avatar Giuseppe Carboni
Browse files

Improved SRP tests

parent 15c6b60a

SRT/Servers/SRTMinorServo/test/Makefile

+1 −1
Original line number Diff line number Diff line
@@ -23,7 +23,7 @@ SRTMinorServoSocketTest_LDFLAGS = -lstdc++ -lpthread 


SRPProgramTrackTest_OBJECTS = SRPProgramTrackTest

SRPProgramTrackTest_CFLAGS = -std=c++17

SRPProgramTrackTest_LIBS = $(GTEST_LIBS) SRTMinorServoSocket SRTMinorServoCommandLibrary IRALibrary ComponentErrors

SRPProgramTrackTest_LIBS = $(GTEST_LIBS) SRTMinorServoSocket SRTMinorServoCommandLibrary IRALibrary ComponentErrors boost_filesystem

SRPProgramTrackTest_LDFLAGS = -lstdc++ -lpthread



# END OF CUSTOMIZATION

SRT/Servers/SRTMinorServo/test/SRPProgramTrackTest.cpp

+172 −40
Original line number Diff line number Diff line
@@ -2,6 +2,8 @@ 
#include <iostream>

#include <thread>

#include <chrono>

#include <ctime>

#include <boost/filesystem.hpp>

#include "SRTMinorServoSocket.h"

#include "SRTMinorServoCommandLibrary.h"
@@ -17,6 +19,7 @@ 
#define TIMEGAP             0.2

#define ADVANCE_TIMEGAP     5

#define EPSILON             0.001

#define MAX_RANGES          std::vector<double>{40, 100, 40, 0.2, 0.2, 0.2}





std::atomic<bool> terminate = false;
@@ -57,9 +60,7 @@ std::string serializeProgramTrack(double timestamp, std::vector<double> coordina 
    std::stringstream stream;

    stream << std::fixed << std::setprecision(6) << timestamp;

    for(double coordinate : coordinates)

    {

        stream << "\t" << coordinate;

    }

    return stream.str();

}
@@ -93,18 +94,14 @@ protected: 
    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++)

        {

            double diff = fabs(first[i] - second[i]);

            if(diff > EPSILON)

            {

                return false;

        }

        }



        return true;

    }
@@ -112,9 +109,18 @@ protected: 
    static void addNoiseToCoordinates(std::vector<double> &coordinates)

    {

        for(size_t i = 0; i < 3; i++)

        {

            coordinates[i] += std::max(-1, std::min(1, rand() % 5 - 2)) * (double(rand() % 100) / 100) * NOISE_THRESHOLD;

    }



    static bool moveAxis(std::vector<double> &coordinates, int axis_to_move, int sign)

    {

        sign = sign / abs(sign);

        double offset_to_add;

        axis_to_move >= 3 ? offset_to_add = 0.05 : offset_to_add = 1.0;

        coordinates[axis_to_move] += sign * offset_to_add;

        if(fabs(coordinates[axis_to_move]) >= MAX_RANGES[axis_to_move])

            return true;

        return false;

    }



    void SetUp() override
@@ -130,15 +136,11 @@ protected: 
        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

    {
@@ -146,7 +148,7 @@ protected: 
    }

};



TEST_F(SRPProgramTrackTest, z_axis_translation)

TEST_F(SRPProgramTrackTest, noise_translation)

{

    SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance();

    SRTMinorServoAnswerMap::iterator iterator;
@@ -155,18 +157,11 @@ TEST_F(SRPProgramTrackTest, z_axis_translation) 


    SRTMinorServoAnswerMap MSStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status());



    /*SRTMinorServoAnswerMap::iterator iter;



    for(iter = MSStatus.begin(); iter != MSStatus.end(); ++iter)

    {

        std::visit([iter](const auto& var) mutable { std::cout << iter->first << ": " << var << std::endl; }, iter->second);

    }*/



    EXPECT_EQ(std::get<std::string>(MSStatus["OUTPUT"]), "GOOD");

    //EXPECT_EQ(std::get<long>(MSStatus["CONTROL"]), 1);

    //EXPECT_EQ(std::get<long>(MSStatus["POWER"]), 1);

    //EXPECT_EQ(std::get<long>(MSStatus["EMERGENCY"]), 2);

    //EXPECT_EQ(std::get<long>(MSStatus["ENABLED"]), 1);

    EXPECT_EQ(std::get<long>(MSStatus["CONTROL"]), 1);

    EXPECT_EQ(std::get<long>(MSStatus["POWER"]), 1);

    EXPECT_EQ(std::get<long>(MSStatus["EMERGENCY"]), 2);

    EXPECT_EQ(std::get<long>(MSStatus["ENABLED"]), 1);



    std::cout << "OK." << std::endl;

    std::cout << "Sending initial SRP STATUS command...";
@@ -176,16 +171,130 @@ TEST_F(SRPProgramTrackTest, z_axis_translation) 
    EXPECT_EQ(std::get<long>(SRPStatus["SRP_STATUS"]), 1);

    EXPECT_EQ(std::get<long>(SRPStatus["SRP_BLOCK"]), 2);



    std::cout << "OK." << std::endl;

    std::cout << "Sending PRESET command...";



    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");



    std::cout << "OK." << std::endl;



    signal(SIGINT, sigintHandler);



    do

    {

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



        SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP"));



        EXPECT_EQ(std::get<std::string>(SRPStatus["OUTPUT"]), "GOOD");

        EXPECT_EQ(std::get<long>(SRPStatus["SRP_OPERATIVE_MODE"]), 40);



        std::cout << serializeStatus(SRPStatus) << std::endl;



        if(terminate)

            return;

    }

    while(!compareCoordinates(startingCoordinates, getCoordinates(SRPStatus)));



    std::time_t tn = std::time(0);

    std::tm* now = std::localtime(&tn);

    std::stringstream directory_ss;

    directory_ss << "NoiseTranslationTest-" << 1900 + now->tm_year << std::setfill('0') << std::setw(2) << now->tm_mon + 1 << now->tm_mday << "-" << now->tm_hour << now->tm_min << now->tm_sec;

    std::string directory = directory_ss.str();

    boost::filesystem::create_directory(directory);



    std::thread t([directory]()

    {

        SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance();

        SRTMinorServoAnswerMap SRPStatus;



        ofstream statusFile;

        statusFile.open(directory + "/status.txt", ios::out);



        long unsigned int counter = 0;



        double tn = CIRATools::getUNIXEpoch();



        while(!terminate)

        {

            SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP"));

            statusFile << serializeStatus(SRPStatus) << std::endl;

            if(counter % 10 == 0)

                std::cout << serializeStatus(SRPStatus) << std::endl;

            counter++;



            tn += 0.01;

            std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, tn - CIRATools::getUNIXEpoch()))));

        }



        statusFile.close();

    });



    double start_time = CIRATools::getUNIXEpoch() + ADVANCE_TIMEGAP;

    std::cout << "PRESET position reached, starting PROGRAMTRACK with start time: " << start_time << std::endl;

    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<long>(SRPStatus["SRP_OPERATIVE_MODE"]), 50);



    ofstream programTrackFile;

    programTrackFile.open(directory + "/trajectory.txt", ios::out);

    programTrackFile << serializeProgramTrack(start_time, programTrackCoordinates) << std::endl;



    /*SRTMinorServoAnswerMap::iterator iter;

    double next_expected_time = start_time;



    while(!terminate)

    {

        next_expected_time += TIMEGAP;



        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();

}



    for(iter = SRPStatus.begin(); iter != SRPStatus.end(); ++iter)

TEST_F(SRPProgramTrackTest, cycle_movement)

{

        std::visit([iter](const auto& var) mutable { std::cout << iter->first << ": " << var << std::endl; }, iter->second);

    }*/

    SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance();

    SRTMinorServoAnswerMap::iterator iterator;



    std::cout << "Sending MS STATUS command...";



    SRTMinorServoAnswerMap MSStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status());



    EXPECT_EQ(std::get<std::string>(MSStatus["OUTPUT"]), "GOOD");

    EXPECT_EQ(std::get<long>(MSStatus["CONTROL"]), 1);

    EXPECT_EQ(std::get<long>(MSStatus["POWER"]), 1);

    EXPECT_EQ(std::get<long>(MSStatus["EMERGENCY"]), 2);

    EXPECT_EQ(std::get<long>(MSStatus["ENABLED"]), 1);



    std::cout << "OK." << std::endl;

    std::cout << "Sending PRESET command...";

    std::cout << "Sending initial SRP STATUS command...";



    SRTMinorServoAnswerMap SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP"));

    EXPECT_EQ(std::get<std::string>(SRPStatus["OUTPUT"]), "GOOD");

    EXPECT_EQ(std::get<long>(SRPStatus["SRP_STATUS"]), 1);

    EXPECT_EQ(std::get<long>(SRPStatus["SRP_BLOCK"]), 2);



    std::cout << "OK." << std::endl;

    std::cout << "Sending all axes to 0...";



    std::vector<double> startingCoordinates = {0, 0, 0, 0, 0, 0};

    SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::preset("SRP", startingCoordinates));
@@ -207,32 +316,39 @@ TEST_F(SRPProgramTrackTest, z_axis_translation) 
        std::cout << serializeStatus(SRPStatus) << std::endl;



        if(terminate)

        {

            return;

    }

    }

    while(!compareCoordinates(startingCoordinates, getCoordinates(SRPStatus)));



    std::thread t([]()

    std::time_t tn = std::time(0);

    std::tm* now = std::localtime(&tn);

    std::stringstream directory_ss;

    directory_ss << "CycleMovementTest-" << 1900 + now->tm_year << std::setfill('0') << std::setw(2) << now->tm_mon + 1 << now->tm_mday << "-" << now->tm_hour << now->tm_min << now->tm_sec;

    std::string directory = directory_ss.str();

    boost::filesystem::create_directory(directory);



    std::thread t([directory]()

    {

        SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance();

        SRTMinorServoAnswerMap SRPStatus;



        ofstream statusFile;

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

        statusFile.open(directory + "/status.txt", ios::out);



        long unsigned int counter = 0;



        double tn = CIRATools::getUNIXEpoch();



        while(!terminate)

        {

            SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("SRP"));

            statusFile << serializeStatus(SRPStatus) << std::endl;

            if(counter % 10 == 0)

            {

                std::cout << serializeStatus(SRPStatus) << std::endl;

            }

            counter++;

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



            tn += 0.01;

            std::this_thread::sleep_for(std::chrono::microseconds((int)round(1000000 * std::max(0.0, tn - CIRATools::getUNIXEpoch()))));

        }



        statusFile.close();
@@ -254,11 +370,15 @@ TEST_F(SRPProgramTrackTest, z_axis_translation) 
    EXPECT_EQ(std::get<long>(SRPStatus["SRP_OPERATIVE_MODE"]), 50);



    ofstream programTrackFile;

    programTrackFile.open("SRPTrajectory.txt", ios::out);

    programTrackFile.open(directory + "/trajectory.txt", ios::out);

    programTrackFile << serializeProgramTrack(start_time, programTrackCoordinates) << std::endl;



    double next_expected_time = start_time;

    unsigned int axis_to_move = 3;

    int sign = 1;



    while(!terminate)

    {

        while(!terminate)

        {

            next_expected_time += TIMEGAP;
@@ -266,10 +386,22 @@ TEST_F(SRPProgramTrackTest, z_axis_translation) 
            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);

            if(moveAxis(programTrackCoordinates, axis_to_move, sign))

                sign = sign * -1;



            SRPStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("SRP", trajectory_id, point_id, programTrackCoordinates));

            EXPECT_EQ(std::get<std::string>(SRPStatus["OUTPUT"]), "GOOD");

            //std::cout << serializeProgramTrack(next_expected_time, programTrackCoordinates) << std::endl;

            programTrackFile << serializeProgramTrack(next_expected_time, programTrackCoordinates) << std::endl;



            if(round(programTrackCoordinates[axis_to_move] * 100) == 0 && sign == 1)

            {

                programTrackCoordinates[axis_to_move] = 0.0;

                break;

            }

        }



        axis_to_move == 5 ? axis_to_move = 0 : axis_to_move++;

    }



    programTrackFile.close();