DataLogger.hpp

Go to the documentation of this file.

#ifndef SPIDAR_WIN32_DATA_LOGGER_HPP
#define SPIDAR_WIN32_DATA_LOGGER_HPP

#include <windows.h>
#include <process.h>

#include <cstdint>
#include <string>
#include <fstream>
#include <iomanip>
#include <vector>

namespace Spidar
{
namespace Win32
{

template <class T1, class T2, class T3>
struct LogData
{
    typedef T1  ValueType;          
    typedef T2  VectorType;         
    typedef T3  QuaternionType;     

    VectorType  position;           
    VectorType  velocity;           

    VectorType  targetForce;        
    VectorType  resultantForce;     

    QuaternionType  rotation;       
    VectorType  angularVelocity;    

    VectorType  targetTorque;       
    VectorType  resultantTorque;    

    ValueType   tension[8];         

    static bool outputLabel(std::ofstream& fout) 
    {
        if (!fout.is_open())    return false;

        fout << "position x \t" << "position y \t" << "position z \t";
     
        fout << "velocity x \t" << "velocity y \t" << "velocity z \t"; 

        fout << "target Force x \t" << "target Force y \t" << "target Force z \t";
        fout << "resultant Force x \t" << "resultant Force y \t" << "resultant Force z \t"; 

        fout << "rotation x \t" << "rotation y \t" << "rotation z \t" << "rotation w \t"; 
        fout << "angular velocity x \t" << "angular velocity y \t" << "angular velocity z \t"; 
     
        fout << "target Torque x \t" << "target Torque y \t" << "target Torque z \t"; 
        fout << "resultant Torque x \t" << "resultant Torque y \t" << "resultant Torque z \t"; 

        for (size_t i=0; i<8; ++i)
        {
            fout << "tension " << i << "\t";
        }
        return true;
    }
    
    bool outputData(std::ofstream& fout)
    {
        if (!fout.is_open())    return false;

        fout << position.x() << "\t" << position.y()  << "\t" << position.z()  << "\t";
        fout << velocity.x() << "\t" << velocity.y()  << "\t" << velocity.z()  << "\t";

        fout << targetForce.x() << "\t" << targetForce.y()  << "\t" << targetForce.z()  << "\t";
        fout << resultantForce.x() << "\t" << resultantForce.y()  << "\t" << resultantForce.z()  << "\t";

        fout << rotation.x << "\t" << rotation.y  << "\t" << rotation.z  << "\t" << rotation.w << "\t";
        fout << angularVelocity.x() << "\t" << angularVelocity.y()  << "\t" << angularVelocity.z()  << "\t";

        fout << targetTorque.x() << "\t" << targetTorque.y()  << "\t" << targetTorque.z()  << "\t";
        fout << resultantTorque.x() << "\t" << resultantTorque.y()  << "\t" << resultantTorque.z()  << "\t";

        for (size_t i=0; i<8; ++i)
        {
            fout << tension[i] << "\t";
        }
        return true;
    }
};

template <class T>
class DataLogger
{
public:
    typedef T DataType;

    DataLogger(size_t size=10000);
    ~DataLogger(void) {};

    bool started(void) const {    return started_;  }

    void start(void);
    void add(const DataType& data);
    void output(const std::string& filename);

private:
    std::vector<LARGE_INTEGER>  timeStamp_;
    std::vector<DataType>       logData_;

    LARGE_INTEGER frequency_;
    LARGE_INTEGER start_;

    bool started_;
    bool busy_;

}; // end of class DataLogger.

template <class T>
DataLogger<T>::DataLogger(size_t size) : started_(false), busy_(false)
{
    logData_.reserve(size);
    timeStamp_.reserve(size);

    ::QueryPerformanceFrequency(&frequency_);
}

template <class T>
void DataLogger<T>::start(void)
{
    started_ = false;

    logData_.clear();
    timeStamp_.clear();
    ::QueryPerformanceCounter(&start_);

    started_ = true;
}

template <class T>
void DataLogger<T>::add(const DataType& data)
{
    if (!started_ || busy_)     return;

    LARGE_INTEGER time;
    ::QueryPerformanceCounter(&time);

    timeStamp_.push_back(time);
    logData_.push_back(data);
}

template <class T>
void DataLogger<T>::output(const std::string& filename)
{
    busy_ = true;

    if (logData_.size()!=timeStamp_.size())    return;

    std::ofstream fout;

    fout.open(filename, std::ios::out);

    // データラベルの出力
    fout << "time [ms]\t";

    DataType::outputLabel(fout);

    fout << std::endl;

    // データの出力
    for (size_t i=0; i<logData_.size(); ++i)
    {
        double time = static_cast<double>(timeStamp_[i].QuadPart - start_.QuadPart) / frequency_.QuadPart * 1000.0;

        fout << std::setprecision(10) << time << "\t";

        logData_[i].outputData(fout);

        fout << std::endl;
    }

    fout.close();

    busy_ = false;
}

} // end of namespace Win32.
} // end of namespace Spidar.

#endif // SPIDAR_WIN32_DATA_LOGGER_HPP

// end of file.