Commit c7a31211 authored by Giuseppe Carboni's avatar Giuseppe Carboni
Browse files

Added combined test and plotting script

parent d054c778

SRT/Servers/SRTMinorServo/test/CombinedProgramTrackTest.cpp

+76 −7
Original line number Diff line number Diff line
@@ -27,11 +27,11 @@ 
#define EPSILON                 0.00001



#define SRP_COORDINATES         std::vector<std::string>{ "SRP_TX", "SRP_TY", "SRP_TZ", "SRP_RX", "SRP_RY", "SRP_RZ" }

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

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



#define DEROTATOR               std::string("GFR1")

#define DEROTATOR_COORDINATES   std::vector<std::string>{ DEROTATOR + "_ROTATION" }

#define DEROTATOR_MAX_RANGES    std::vector<double>{ 220 }

#define DEROTATOR_RANGES        std::vector<double>{ 10.0, 50.0 }





std::atomic<bool> terminate = false;
@@ -342,15 +342,84 @@ protected: 
    }

};



TEST_F(CombinedProgramTrackTest, CombinedProgramTrackTest)

TEST_F(CombinedProgramTrackTest, SineWaveMovementTest)

{

    std::this_thread::sleep_for(std::chrono::seconds(5));

    terminate = true;

    SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance();

    SRTMinorServoAnswerMap SRPStatus, DerotatorStatus;



    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> SRPCoordinates = SRPStartingCoordinates;

    std::vector<double> DerotatorCoordinates = DerotatorStartingCoordinates;



    std::vector<double> phase_shift;

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

    {

        phase_shift.push_back((double)std::rand() / RAND_MAX * 60);

        SRPCoordinates[axis] = SRP_MAX_RANGES[axis] * sin(phase_shift[axis] * 2 * M_PI / 60);

    }



    phase_shift.push_back((double)std::rand() / RAND_MAX * 60);

    double derotator_amplitude = (DEROTATOR_RANGES[1] - DEROTATOR_RANGES[0]) / 2;

    double derotator_center = (DEROTATOR_RANGES[0] + DEROTATOR_RANGES[1]) / 2;

    DerotatorCoordinates[0] = derotator_center + derotator_amplitude * sin(phase_shift[3] * 2 * M_PI / 60);



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

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

    DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("Derotatore" + DEROTATOR, trajectory_id, point_id, DerotatorCoordinates, start_time));

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



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



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



    DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::status("Derotatore" + DEROTATOR));

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

    EXPECT_EQ(std::get<long>(DerotatorStatus[DEROTATOR + "_OPERATIVE_MODE"]), 50);



    ofstream SRPProgramTrackFile;

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

    SRPProgramTrackFile << CombinedProgramTrackTest::serializeCoordinates(start_time, SRPCoordinates) << std::endl;



    ofstream DerotatorProgramTrackFile;

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

    DerotatorProgramTrackFile << CombinedProgramTrackTest::serializeCoordinates(start_time, DerotatorCoordinates) << std::endl;



    double next_expected_time = start_time;



    while(!terminate)

    {

        next_expected_time += TIMEGAP;

        double time_delta = next_expected_time - start_time;



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

        point_id++;



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

        {

            SRPCoordinates[axis] = SRP_MAX_RANGES[axis] * sin((time_delta + phase_shift[axis]) * 2 * M_PI / 60);

        }

        DerotatorCoordinates[0] = derotator_center + derotator_amplitude * sin((time_delta + phase_shift[3]) * 2 * M_PI / 60);



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

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

        SRPProgramTrackFile << CombinedProgramTrackTest::serializeCoordinates(next_expected_time, SRPCoordinates) << std::endl;



        DerotatorStatus = socket.sendCommand(SRTMinorServoCommandLibrary::programTrack("Derotatore" + DEROTATOR, trajectory_id, point_id, DerotatorCoordinates));

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

        DerotatorProgramTrackFile << CombinedProgramTrackTest::serializeCoordinates(next_expected_time, DerotatorCoordinates) << std::endl;

    }



    SRPProgramTrackFile.close();

    DerotatorProgramTrackFile.close();

    SRPStatusThread.join();

    DerotatorStatusThread.join();

    return;



    /*SRTMinorServoSocket& socket = SRTMinorServoSocket::getInstance();

    /*

    SRTMinorServoAnswerMap SRPStatus;



    ofstream programTrackFile;

SRT/Servers/SRTMinorServo/test/TESTS/plotCombined.py

0 → 100755
+112 −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()



srp_status_time = []

derotator_status_time = []

status_tx = []

status_ty = []

status_tz = []

status_rx = []

status_ry = []

status_rz = []

status_rotation = []

starttime = None



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

    for line in statusfile:

        args = line.strip().split()

        if not starttime:

            starttime = float(args[0])

        srp_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]))



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

    for line in statusfile:

        args = line.strip().split()

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

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



trajectory_time = []

trajectory_tx = []

trajectory_ty = []

trajectory_tz = []

trajectory_rx = []

trajectory_ry = []

trajectory_rz = []

trajectory_rotation = []



with open(os.path.join(arguments.directory, 'SRP', '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]))



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

    for line in trajectoryfile:

        args = line.strip().split()

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



fig, axs = plt.subplots(7)

fig.canvas.manager.set_window_title('Program Track Positions')

fig.suptitle('Program Track Positions')



axs[0].plot(srp_status_time, status_tx, 'r')

axs[0].plot(trajectory_time, trajectory_tx, 'b')

axs[0].set_ylabel('tx (mm)')

axs[0].sharey(axs[1])



axs[1].plot(srp_status_time, status_ty, 'r')

axs[1].plot(trajectory_time, trajectory_ty, 'b')

axs[1].set_ylabel('ty (mm)')

axs[1].sharey(axs[2])



axs[2].plot(srp_status_time, status_tz, 'r')

axs[2].plot(trajectory_time, trajectory_tz, 'b')

axs[2].set_ylabel('tz (mm)')



axs[3].plot(srp_status_time, status_rx, 'r')

axs[3].plot(trajectory_time, trajectory_rx, 'b')

axs[3].set_ylabel('rx (deg)')

axs[3].sharey(axs[4])



axs[4].plot(srp_status_time, status_ry, 'r')

axs[4].plot(trajectory_time, trajectory_ry, 'b')

axs[4].set_ylabel('ry (deg)')

axs[4].sharey(axs[5])



axs[5].plot(srp_status_time, status_rz, 'r')

axs[5].plot(trajectory_time, trajectory_rz, 'b')

axs[5].set_ylabel('rz (deg)')



axs[6].plot(derotator_status_time, status_rotation, 'r')

axs[6].plot(trajectory_time, trajectory_rotation, 'b')

axs[6].set_ylabel('derot (deg)')

axs[6].set_xlabel('time (s)')





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

try:

    plt.show()

except KeyboardInterrupt:

    pass