Commit 94a3e593 authored by Giuseppe Carboni's avatar Giuseppe Carboni
Browse files

Updated SRPProgramTrackTest 

Added a script to plot trajectories
parent 207d1f48

SRT/Servers/SRTMinorServo/test/SRPProgramTrackTest.cpp

+128 −183
Original line number Diff line number Diff line
@@ -10,27 +10,60 @@ 
// 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("192.168.200.13")

//#define PORT                4758

#define ADDRESS             std::string("127.0.0.1")

#define PORT                12800

#define SOCKET_TIMEOUT      0.1

//#define ADDRESS             std::string("192.168.200.13")

//#define PORT                4758

#define SOCKET_TIMEOUT      0.5

#define NOISE_THRESHOLD     1

#define TIMEGAP             0.2

#define ADVANCE_TIMEGAP     5

#define EPSILON             0.001

#define EPSILON             0.00001

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



#define STATUS_PERIOD       0.01



std::atomic<bool> terminate = false;



void sigintHandler(int sig_num)



class SRPProgramTrackTest : public ::testing::Test

{

protected:

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



    static void printStatus(std::string filename)

    {

        SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance();

        SRTMinorServoAnswerMap SRPStatus;



        ofstream statusFile;

        statusFile.open(filename, ios::out);



        long unsigned int counter = 0;



        double tn = CIRATools::getUNIXEpoch();



        while(!terminate)

        {

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

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

            if(counter % 10 == 0)

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

            counter++;



            tn += STATUS_PERIOD;

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

        }



        statusFile.close();

    }



    static void sigintHandler(int sig_num)

    {

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

        terminate = true;

    }



std::string serializeStatus(SRTMinorServoAnswerMap map)

    static std::string serializeStatus(SRTMinorServoAnswerMap map)

    {

        std::stringstream stream;

        stream << std::fixed << std::setprecision(6);
@@ -55,7 +88,7 @@ std::string serializeStatus(SRTMinorServoAnswerMap map) 
        return stream.str();

    }



std::string serializeProgramTrack(double timestamp, std::vector<double> coordinates)

    static std::string serializeProgramTrack(double timestamp, std::vector<double> coordinates)

    {

        std::stringstream stream;

        stream << std::fixed << std::setprecision(6) << timestamp;
@@ -64,9 +97,6 @@ std::string serializeProgramTrack(double timestamp, std::vector<double> coordina 
        return stream.str();

    }



class SRPProgramTrackTest : public ::testing::Test

{

protected:

    static std::vector<double> getCoordinates(SRTMinorServoAnswerMap SRPStatus)

    {

        std::vector<double> currentCoordinates;
@@ -140,65 +170,74 @@ protected: 
            else

                FAIL() << "Unexpected failure." << std::endl;

        }

    }



    void TearDown() override

    {

        SRTMinorServoSocket::destroyInstance();

        terminate = false;

    }

};



TEST_F(SRPProgramTrackTest, noise_translation)

{

        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;



        SRTMinorServoAnswerMap::iterator iterator;

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

        {

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

        }



        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 PRESET command...";



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

        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::cout << "Sending all axes to 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);

        signal(SIGINT, SRPProgramTrackTest::sigintHandler);



    do

        while(true)

        {

            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(!compareCoordinates(startingCoordinates, getCoordinates(SRPStatus)))

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

            else

                break;



            if(terminate)

            return;

                FAIL() << "Aborting test..." << std::endl;

        }



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

    }



    void TearDown() override

    {

        SRTMinorServoSocket::destroyInstance();

        terminate = false;

    }

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

};



TEST_F(SRPProgramTrackTest, noise_translation)

{

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

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

    std::stringstream directory_ss;
@@ -206,32 +245,7 @@ TEST_F(SRPProgramTrackTest, noise_translation) 
    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();

    });

    std::thread t(&SRPProgramTrackTest::printStatus, directory + "/status.txt");



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

    std::cout << "PRESET position reached, starting PROGRAMTRACK with start time: " << start_time << std::endl;
@@ -239,6 +253,8 @@ TEST_F(SRPProgramTrackTest, noise_translation) 
    unsigned int point_id = 0;

    std::vector<double> programTrackCoordinates = startingCoordinates;



    SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance();

    SRTMinorServoAnswerMap SRPStatus;

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

    EXPECT_EQ(std::get<std::string>(SRPStatus["OUTPUT"]), "GOOD");
@@ -250,7 +266,7 @@ TEST_F(SRPProgramTrackTest, noise_translation) 


    ofstream programTrackFile;

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

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

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



    double next_expected_time = start_time;
@@ -264,7 +280,7 @@ TEST_F(SRPProgramTrackTest, noise_translation) 
        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 << SRPProgramTrackTest::serializeProgramTrack(next_expected_time, programTrackCoordinates) << std::endl;

    }



    programTrackFile.close();
@@ -273,54 +289,6 @@ TEST_F(SRPProgramTrackTest, noise_translation) 


TEST_F(SRPProgramTrackTest, cycle_movement)

{

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

    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;
@@ -328,32 +296,7 @@ TEST_F(SRPProgramTrackTest, cycle_movement) 
    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();

    });

    std::thread t(&SRPProgramTrackTest::printStatus, directory + "/status.txt");



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

    std::cout << "PRESET position reached, starting PROGRAMTRACK with start time: " << start_time << std::endl;
@@ -361,6 +304,8 @@ TEST_F(SRPProgramTrackTest, cycle_movement) 
    unsigned int point_id = 0;

    std::vector<double> programTrackCoordinates = startingCoordinates;



    SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance();

    SRTMinorServoAnswerMap SRPStatus;

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

    EXPECT_EQ(std::get<std::string>(SRPStatus["OUTPUT"]), "GOOD");
@@ -372,7 +317,7 @@ TEST_F(SRPProgramTrackTest, cycle_movement) 


    ofstream programTrackFile;

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

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

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



    double next_expected_time = start_time;

    unsigned int axis_to_move = 0;
@@ -406,8 +351,8 @@ TEST_F(SRPProgramTrackTest, cycle_movement) 


            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;

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

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



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

            {

SRT/Servers/SRTMinorServo/test/plotTrajectories.py

0 → 100755
+88 −0
Original line number Diff line number Diff line 
#!/usr/bin/env python

import os

import numpy as np

import matplotlib.pyplot as plt

from argparse import ArgumentParser



parser = ArgumentParser()

parser.add_argument(

    'directory',

    help="Directory containing the 'status.txt' and 'trajectory.txt' files"

)



arguments = parser.parse_args()



status_time = []

status_tx = []

status_ty = []

status_tz = []

status_rx = []

status_ry = []

status_rz = []

starttime = None



with open(os.path.join(arguments.directory, 'status.txt'), 'r') as statusfile:

    for line in statusfile:

        args = line.strip().split()

        if not starttime:

            starttime = float(args[0])

        status_time.append(float(args[0]) - starttime)

        status_tx.append(float(args[1]))

        status_ty.append(float(args[2]))

        status_tz.append(float(args[3]))

        status_rx.append(float(args[4]))

        status_ry.append(float(args[5]))

        status_rz.append(float(args[6]))



trajectory_time = []

trajectory_tx = []

trajectory_ty = []

trajectory_tz = []

trajectory_rx = []

trajectory_ry = []

trajectory_rz = []



with open(os.path.join(arguments.directory, 'trajectory.txt'), 'r') as trajectoryfile:

    for line in trajectoryfile:

        args = line.strip().split()

        trajectory_time.append(float(args[0]) - starttime)

        trajectory_tx.append(float(args[1]))

        trajectory_ty.append(float(args[2]))

        trajectory_tz.append(float(args[3]))

        trajectory_rx.append(float(args[4]))

        trajectory_ry.append(float(args[5]))

        trajectory_rz.append(float(args[6]))



fig = plt.figure()

fig.canvas.manager.set_window_title('SRP Positions')

fig.suptitle('SRP Positions')

gs = fig.add_gridspec(3, 2, hspace=0)

((tx, rx), (ty, ry), (tz, rz)) = gs.subplots(sharex=True, sharey='col')

tx.plot(status_time, status_tx, 'r')

tx.plot(trajectory_time, trajectory_tx, 'b')

tx.set_ylabel('tx (mm)')



ty.plot(status_time, status_ty, 'r')

ty.plot(trajectory_time, trajectory_ty, 'b')

ty.set_ylabel('ty (mm)')



tz.plot(status_time, status_tz, 'r')

tz.plot(trajectory_time, trajectory_tz, 'b')

tz.set_xlabel('time (s)')

tz.set_ylabel('tz (mm)')



rx.plot(status_time, status_rx, 'r')

rx.plot(trajectory_time, trajectory_rx, 'b')

rx.set_ylabel('rx (deg)')



ry.plot(status_time, status_ry, 'r')

ry.plot(trajectory_time, trajectory_ry, 'b')

ry.set_ylabel('ry (deg)')



rz.plot(status_time, status_rz, 'r')

rz.plot(trajectory_time, trajectory_rz, 'b')

rz.set_xlabel('time (s)')

rz.set_ylabel('rz (deg)')



plt.get_current_fig_manager().window.attributes('-zoomed', True)

plt.show()