SmoothDerivative_Test.cpp

Go to the documentation of this file.

#include <iostream>
#include <gtest/gtest.h>
#include "robodyn_utilities/SmoothDerivative.h"
#include <ros/package.h>
#include <stdexcept>
#include <math.h>
#include <fstream>

class SmoothDerivativeTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        ASSERT_NO_THROW(sd = new SmoothDerivative());
    }

    virtual void TearDown()
    {
    }

    SmoothDerivative* sd;

};


TEST_F(SmoothDerivativeTest, initialize)
{
    EXPECT_FALSE(sd->isInitialized);

    std::vector<double> a_0;
    std::vector<double> a_smoothing;

    EXPECT_TRUE(sd->Initialize(a_0, a_smoothing, 350));

    EXPECT_TRUE(sd->isInitialized);

    a_0.push_back(10);

    EXPECT_FALSE(sd->Initialize(a_0, a_smoothing, 350));
    EXPECT_FALSE(sd->isInitialized);

    a_smoothing.push_back(0.2);

    EXPECT_TRUE(sd->Initialize(a_0, a_smoothing, 350));

}

TEST_F(SmoothDerivativeTest, OnExecute)
{
    std::vector<double> a_0;
    std::vector<double> a;
    std::vector<double> adot;
    std::vector<double> a_smoothing;

    a_0.push_back(0);
    a_smoothing.push_back(0.2);

    EXPECT_THROW(sd->OnExecute(a, adot), std::runtime_error);

    sd->Initialize(a_0, a_smoothing, 1);

    //size mismatch, a = size 0, but initilization vals = 1
    EXPECT_THROW(sd->OnExecute(a, adot), std::runtime_error);

    a.push_back(0);

    EXPECT_NO_THROW(sd->OnExecute(a, adot));

    for (int i = 1; i < 100; ++i)
    {
        a[0] = i * 3;

        EXPECT_NO_THROW(sd->OnExecute(a, adot));

        EXPECT_EQ(1, a.size());
        EXPECT_EQ(1, adot.size());
        //EXPECT_NEAR(3, adot[0], 3/i);

    }

    double oldvel = a[0];
    EXPECT_FLOAT_EQ(3, adot[0]);

    for (int i = 1; i < 10000; ++i)
    {
        a[0] = oldvel;

        EXPECT_NO_THROW(sd->OnExecute(a, adot));

        EXPECT_EQ(1, a.size());
        EXPECT_EQ(1, adot.size());
        //EXPECT_NEAR(0, adot[0], 3/i);
    }

    EXPECT_NEAR(0, adot[0], 0.001);

}
/*
TEST_F(SmoothDerivativeTest, outputData)
{
    std::vector<double> a(5, 0.);
    std::vector<double> adot(5, 0.8);
    std::vector<double> a_smoothing(5, 0.);

    a_smoothing[1] = 0.85;
    a_smoothing[2] = 0.9;
    a_smoothing[3] = 0.95;
    a_smoothing[4] = 0.98;

    sd->Initialize(a, a_smoothing, 350);

    std::ofstream file;
    file.open("test.csv");
    file << "t, p, dp, v0, v2, v5, v8, v10" << std::endl;
    file << "0, 0, 0, 0, 0, 0, 0, 0" << std::endl;

    unsigned int cycle = 0;
    unsigned int minCycles = 50;
    unsigned int maxCycles = 5000;
    double eps = 0.0001;
    double t = 0.;
    double prev = 0.;

    // constant
    cycle = 0;
    while ((fabs(adot[0]) > eps || fabs(adot[1]) > eps || fabs(adot[2]) > eps || fabs(adot[3]) > eps || fabs(adot[4]) > eps ||cycle < minCycles) && cycle < maxCycles)
    {
        t += 1./350.;
        EXPECT_NO_THROW(sd->OnExecute(a,adot));
        file << t << ", " << a[0] << ", " << (a[0] - prev) * 350. << ", " << adot[0] << ", " << adot[1] << ", " << adot[2] << ", " << adot[3] << ", " << adot[4] << std::endl;
        ++cycle;
        prev = a[0];
    }

    // step input
    a[0] = a[1] = a[2] = a[3] = a[4] = static_cast<double>(static_cast<long>(50.*57.33))/57.33;
    cycle = 0;
    while ((fabs(adot[0]) > eps || fabs(adot[1]) > eps || fabs(adot[2]) > eps || fabs(adot[3]) > eps || fabs(adot[4]) > eps ||cycle < minCycles) && cycle < maxCycles)
    {
        t += 1./350.;
        EXPECT_NO_THROW(sd->OnExecute(a,adot));
        file << t << ", " << a[0] << ", " << (a[0] - prev) * 350. << ", " << adot[0] << ", " << adot[1] << ", " << adot[2] << ", " << adot[3] << ", " << adot[4] << std::endl;
        ++cycle;
        prev = a[0];
    }

    // impulse input
    cycle = 0;
    while ((fabs(adot[0]) > eps || fabs(adot[1]) > eps || fabs(adot[2]) > eps || fabs(adot[3]) > eps || fabs(adot[4]) > eps ||cycle < minCycles) && cycle < maxCycles)
    {
        t += 1./350.;
        if (cycle == 0)
        {
            a[0] = a[1] = a[2] = a[3] = a[4] = 0.;
        }
        else
        {
            a[0] = a[1] = a[2] = a[3] = a[4] = static_cast<double>(static_cast<long>(50.*57.33))/57.33;
        }
        EXPECT_NO_THROW(sd->OnExecute(a,adot));
        file << t << ", " << a[0] << ", " << (a[0] - prev) * 350. << ", " << adot[0] << ", " << adot[1] << ", " << adot[2] << ", " << adot[3] << ", " << adot[4] << std::endl;
        ++cycle;
        prev = a[0];
    }

    // sin wave
    cycle = 0;
    while (cycle < maxCycles)
    {
        t += 1./350.;
        a[0] = a[1] = a[2] = a[3] = a[4] = static_cast<double>(static_cast<long>(57.33 * cos(cycle * 3.14159/350)*50.))/57.33;
        EXPECT_NO_THROW(sd->OnExecute(a,adot));
        file << t << ", " << a[0] << ", " << (a[0] - prev) * 350. << ", " << adot[0] << ", " << adot[1] << ", " << adot[2] << ", " << adot[3] << ", " << adot[4] << std::endl;
        ++cycle;
        prev = a[0];
    }
    file.close();
}*/

int main(int argc, char** argv)
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}