From c41200cb6a6880feec75bb84ee5ae99360180735 Mon Sep 17 00:00:00 2001 From: Frank Ebner Date: Fri, 25 Dec 2015 10:07:48 +0100 Subject: [PATCH] initial commit -converter .txt -> MatLab matrices --- workspace/CMakeLists.txt | 85 +++++ workspace/Interpolator.h | 58 ++++ workspace/conv.cpp | 55 +++ workspace/main.cpp | 431 ++++++++++++++++++++++++ workspace/sensors/Recording.h | 21 ++ workspace/sensors/SensorAccelerometer.h | 25 ++ workspace/sensors/SensorGyro.h | 25 ++ workspace/sensors/SensorMagneticField.h | 25 ++ workspace/sensors/SensorReader.h | 129 +++++++ workspace/sensors/SensorReadings.h | 29 ++ workspace/sensors/Sensors.h | 19 ++ 11 files changed, 902 insertions(+) create mode 100644 workspace/CMakeLists.txt create mode 100644 workspace/Interpolator.h create mode 100644 workspace/conv.cpp create mode 100644 workspace/main.cpp create mode 100644 workspace/sensors/Recording.h create mode 100644 workspace/sensors/SensorAccelerometer.h create mode 100644 workspace/sensors/SensorGyro.h create mode 100644 workspace/sensors/SensorMagneticField.h create mode 100644 workspace/sensors/SensorReader.h create mode 100644 workspace/sensors/SensorReadings.h create mode 100644 workspace/sensors/Sensors.h diff --git a/workspace/CMakeLists.txt b/workspace/CMakeLists.txt new file mode 100644 index 0000000..2a1e234 --- /dev/null +++ b/workspace/CMakeLists.txt @@ -0,0 +1,85 @@ +# Usage: +# Create build folder, like RC-build next to RobotControl and WifiScan folder +# CD into build folder and execute 'cmake -DCMAKE_BUILD_TYPE=Debug ../RobotControl' +# make + +CMAKE_MINIMUM_REQUIRED(VERSION 2.8) + +# select build type +SET( CMAKE_BUILD_TYPE "${CMAKE_BUILD_TYPE}" ) + +PROJECT(HandyGames) + +IF(NOT CMAKE_BUILD_TYPE) + MESSAGE(STATUS "No build type selected. Default to Debug") + SET(CMAKE_BUILD_TYPE "Debug") +ENDIF() + + + +INCLUDE_DIRECTORIES( + ../ + /apps/workspaces +) + + +FILE(GLOB HEADERS + ./*.h + ./*/*.h + ./*/*/*.h + ./*/*/*/*.h + ./*/*/*/*/*.h + ./*/*/*/*/*/*.h +) + +FILE(GLOB SOURCES + ./*.cpp + ../KLib/inc/tinyxml/tinyxml2.cpp +) + + +if(${CMAKE_GENERATOR} MATCHES "Visual Studio") + + SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} /D_X86_ /D_USE_MATH_DEFINES") + SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /Zi /Oi /GL /Ot /Ox /D_X86_ /D_USE_MATH_DEFINES") + SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /DEBUG") + SET(CMAKE_EXE_LINKER_FLAGS_RELEASE "${CMAKE_EXE_LINKER_FLAGS_RELEASE} /LTCG /INCREMENTAL:NO") + + set(CMAKE_CONFIGURATION_TYPES Release Debug) + +else() + +# system specific compiler flags +ADD_DEFINITIONS( + -g + -std=gnu++11 + -Wall + -Werror=return-type + -Wextra + #-O2 +) + +endif() + + +find_package(OpenMP) +if (OPENMP_FOUND) + set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +endif() + + +# build a binary file +ADD_EXECUTABLE( + ${PROJECT_NAME} + ${HEADERS} + ${SOURCES} +) + +# needed external libraries +TARGET_LINK_LIBRARIES( + ${PROJECT_NAME} +) + +SET(CMAKE_C_COMPILER ${CMAKE_CXX_COMPILER}) + diff --git a/workspace/Interpolator.h b/workspace/Interpolator.h new file mode 100644 index 0000000..3cd2e8b --- /dev/null +++ b/workspace/Interpolator.h @@ -0,0 +1,58 @@ +#ifndef INTERPOLATOR_H +#define INTERPOLATOR_H + +#include +#include + + +namespace K { + + /** + * this class allows adding values with some sort of key. + * hereafter it is possible to interpolate between two values + * using a provided key. + */ + template class Interpolator { + + /** combine key and value within one struct */ + struct KeyedEntry { + Key key; + Value val; + KeyedEntry(const Key key, const Value& val) : key(key), val(val) {;} + }; + + public: + + /** all entries within the interpolator */ + std::vector values; + + public: + + /** add a new timed entry */ + void add(const Key key, const Value& val) { + values.push_back(KeyedEntry(key, val)); + } + + /** get the interpolated value for the given key */ + Value get(const Key key) const { + auto comp = [] (const Key& key, const KeyedEntry& e) {return key < e.key;}; + auto it = std::upper_bound(values.begin(), values.end(), key, comp); // first element > key + const KeyedEntry eH = *it; // greater than key + const KeyedEntry eL = *(--it); // smaller than key + const Key diff = eH.key - eL.key; // distance between upper and lower bound + const float vL = float(eH.key - key) / float(diff); // influence factor for the smaller one + const float vH = float(key - eL.key) / float(diff); // influence factor for the larger one + return (eH.val * vH) + (eL.val * vL); // interpolate + } + + /** ensure the first key starts at 0 */ + void makeRelative() { + const Key firstKey = values[0].key; + for (KeyedEntry& e : values) {e.key -= firstKey;} + } + + }; + +} + +#endif // INTERPOLATOR_H diff --git a/workspace/conv.cpp b/workspace/conv.cpp new file mode 100644 index 0000000..19fb4f0 --- /dev/null +++ b/workspace/conv.cpp @@ -0,0 +1,55 @@ + +#include "sensors/SensorReader.h" +#include "Interpolator.h" +#include + +/** the step size to use for interpolating the output (in ms) */ +static constexpr int stepSizeMS = 10; + +/** interpolate and convert the readings for one sensor to a matLab matrix */ +template std::string toMatLab(const SensorReadings& values) { + + // create and feed the interpolator with the timed sensor readings + K::Interpolator interpol; + for(const auto& reading : values.values) {interpol.add(reading.ts, reading.val);} + interpol.makeRelative(); + + // create interpolated output + const int lengthMS = interpol.values.back().key; + std::stringstream ss; + ss << "[" << std::endl; + for (int ms = stepSizeMS; ms < lengthMS; ms += stepSizeMS) { + const T cur = interpol.get(ms); + ss << cur.x << " " << cur.y << " " << cur.z << std::endl; + } + ss << "];" << std::endl; + + return ss.str(); + +} + + +int main(const int argc, const char** argv) { + + std::cout << "converting " << (argc-1) << " files" << std::endl; + + for (int i = 1; i < argc; ++i) { + + std::string fileIn = argv[i]; + std::string fileOut = fileIn + ".m"; + + // read all sensor values within the input file + Recording rec = SensorReader::read(fileIn); + + // convert them to MatLab matrices + std::ofstream out(fileOut); + out << "Accel = " << toMatLab(rec.accel); + out << "Gyro = " << toMatLab(rec.gyro); + out << "Magnet = " << toMatLab(rec.magField); + out.close(); + + } + + return 0; + +} diff --git a/workspace/main.cpp b/workspace/main.cpp new file mode 100644 index 0000000..fdf1280 --- /dev/null +++ b/workspace/main.cpp @@ -0,0 +1,431 @@ + + + +//#include "sensors/SensorReader.h" +//#include "Interpolator.h" +//#include +//#include +//#include +//#include + +//#include +//#include + +//enum class PracticeType { +// REST, +// JUMPING_JACK, +// SITUPS, +// PUSHUPS, +// REJECT, +//}; + +///** interpolate the output for the given position using the provided range */ +//template T blur(K::Interpolator& interpol, const uint64_t ms, const int s = 3) { +// return interpol.get(ms-s*2) * 0.1 + +// interpol.get(ms-s) * 0.2 + +// interpol.get(ms) * 0.4 + +// interpol.get(ms+s) * 0.2 + +// interpol.get(ms+s*2) * 0.1; +//} + +//struct Practice { + +// PracticeType type; +// Recording rec; +// std::vector keyGyro; + +// //Practice(const PracticeType p, const Recording& rec, const std::vector& keyGyro) : p(p), rec(rec), keyGyro(keyGyro) {;} + +// K::Interpolator getInterpol() { +// K::Interpolator interpol; +// for (auto it : rec.gyro.values) {interpol.add(it.ts, it.val);} +// interpol.makeRelative(); +// return interpol; +// } + +//}; + +//static constexpr int NUM_IN = 60; +//static constexpr int NUM_HID = 16; +//static constexpr int NUM_OUT = 4; +//static constexpr int NUM_ARGS = NUM_IN*NUM_HID + NUM_HID*NUM_OUT; + +//static std::vector getNetworkInput(K::Interpolator& interpol, const uint64_t pos) { + +// std::vector val; +// val.resize(NUM_IN); +// int idx = 0; + +// for (int offset = -500; offset < 500; offset += 50) { +// SensorGyro gyro = interpol.get(pos + offset); +// val[idx++] = gyro.x; +// val[idx++] = gyro.y; +// val[idx++] = gyro.z; +// assert(idx <= NUM_IN); +// } + +// return val; + +//} + +///** get the index of the largest element within vec */ +//static int getMaxIdx(const K::NeuralNetResultIHO& vec) { +// float max = 0; +// int idx = 0; +// for (int i = 0; i < NUM_OUT; ++i) { +// if (vec.values[i] > max) { +// max = vec.values[i]; +// idx = i; +// } +// } +// return idx; +//} + +//struct TMP {int index; float value;}; +//static std::vector getSorted(const K::NeuralNetResultIHO& vec) { +// std::vector tmp; +// for (int i = 0; i < NUM_OUT; ++i) {tmp.push_back( TMP{i, vec.values[i]} );} +// auto comp = [] (const TMP& t1, const TMP& t2) {return t2.value < t1.value;}; +// std::sort(tmp.begin(), tmp.end(), comp); +// return tmp; +//} + +//static void debug(Practice& p, K::NeuralNetResultIHO& res) { +// const int maxIdx = getMaxIdx(res); +// const char max = (res.values[maxIdx] > 0.5) ? (maxIdx + '0') : ('?'); +// std::cout << "practice was: " << (int)p.type; +// std::cout << " network says: " << max << "\t"; +// std::cout << "["; +// for (int i = 0; i < NUM_OUT; ++i) { +// std::cout << res.values[i] << ", "; +// } +// std::cout << "]" << std::endl; +//} + +//static void debugPlot(Practice& p) { + +// static K::Gnuplot gp; +// K::GnuplotPlot plot; +// K::GnuplotPlotElementLines line[3]; + +// line[0].setColorHex("#ff0000"); line[0].setTitle("x"); +// line[1].setColorHex("#00ff00"); line[1].setTitle("y"); +// line[2].setColorHex("#0000ff"); line[2].setTitle("z"); + +// plot.add(&line[0]); +// plot.add(&line[1]); +// plot.add(&line[2]); + +// K::Interpolator interpol = p.getInterpol(); + +// for (int ms = 0; ms < 20000; ms += 50) { +// SensorGyro s = interpol.get(ms); +// line[0].add(K::GnuplotPoint2(ms, s.x)); +// line[1].add(K::GnuplotPoint2(ms, s.y)); +// line[2].add(K::GnuplotPoint2(ms, s.z)); +// } + +// gp.setDebugOutput(true); +// gp.draw(plot); +// gp.flush(); + +//} + + +//int main(void) { + +// std::vector practices; + +// practices.push_back( +// Practice { +// PracticeType::JUMPING_JACK, +// SensorReader::read("/mnt/firma/kunden/HandyGames/daten/jumpingjack/jumpingjack_gl_5_subject_3_left.txt"), +// {1950, 2900, 3850, 4850, 5850, 6850, 7850, 8850, 9800, 10800, 11850} +// } +// ); + + + +// practices.push_back( +// Practice { +// PracticeType::REST, +// SensorReader::read("/mnt/firma/kunden/HandyGames/daten/idle/restposition_gl_24.txt"), +// {1000, 2000, 3000, 4000, 5000, 6000, 7000, 8000, 9000} +// } +// ); + +// practices.push_back( +// Practice { +// PracticeType::SITUPS, +// SensorReader::read("/mnt/firma/kunden/HandyGames/daten/situps/situps_gl_12_subject_1_left.txt"), +// {1850, 3250, 4750, 6150, 7550, 8950, 10350, 11600, 13000} +// } +// ); + +// practices.push_back( +// Practice { +// PracticeType::PUSHUPS, +// SensorReader::read("/mnt/firma/kunden/HandyGames/daten/pushups/pushups_gl_8_subject_4_right.txt"), +// {2750, 4200, 5850, 7400, 9000, 10650} +// //{3500, 5000, 8300, 9900, 11550} +// } +// ); + + + +// practices.push_back( +// Practice { +// PracticeType::REST, +// SensorReader::read("/mnt/firma/kunden/HandyGames/daten/jumpingjack/jumpingjack_gl_5_subject_3_left.txt"), +// {1950+500, 2900+500, 3850+500, 4850+500, 5850+500, 6850+500, 7850+500, 8850+500, 9800+500, 10800+500, 11850+500} +// } +// ); +//// practices.push_back( +//// Practice { +//// PracticeType::REST, +//// SensorReader::read("/mnt/firma/kunden/HandyGames/daten/pushups/pushups_gl_8_subject_4_right.txt"), +//// //{2750, 4200, 5850, 7400, 9000, 10650} +//// {3500, 5000, 8300, 9900, 11550} +//// } +//// ); +// practices.push_back( +// Practice { +// PracticeType::REST, +// SensorReader::read("/mnt/firma/kunden/HandyGames/daten/situps/situps_gl_12_subject_1_left.txt"), +// {1850+600, 3250+600, 4750+600, 6150+600, 7550+600, 8950+600, 10350+600, 11600+600, 13000+600} +// } +// ); + + +// debugPlot(practices.back()); +// sleep(100); + +// class MyOpt : public K::NumOptFunction { + +// public: + +// std::vector& practices; +// K::NeuralNetIHO& net; + +// /** ctor */ +// MyOpt(std::vector& practices, K::NeuralNetIHO& net) : practices(practices), net(net) { +// ; +// } + +// double getValue(const K::NumOptVector& args) const { + +// // configure the network +// std::vector vals; +// for(int i = 0; i < NUM_ARGS; ++i) {vals.push_back(args[i]);} +// net.setAll(vals); + +// // temporals +// float points = 0; + +// // process every practice +// for (Practice& p : practices) { + +// // get the values for the neural-net-input + +// K::Interpolator interpol = p.getInterpol(); + +// // process 4 (positive) occurences within the practice +// for (int key = 0; key < 4; ++key) { + +// for (int o = -100; o <= +100; o +=50) { + +// const uint64_t ts = p.keyGyro[key] + o; +// const std::vector values = getNetworkInput(interpol, ts); + +// // calculate the output +// const K::NeuralNetResultIHO res = net.getOutput(values.data()); + + +// // largest value matches the desired type -> good! +// std::vector resSort = getSorted(res); +// if (resSort[0].index == (int) p.type) { +// //if ( (resSort[0].value - resSort[1].value) > 0.25 ) { +// ++points; +// points += resSort[0].value; +// points -= resSort[1].value; +// //} +// //points += resSort[0].value; +// //points += (resSort[0].value - resSort[1].value); +// } else { +// --points; +// } + +//// // update the score +//// for (int i = 0; i < NUM_OUT; ++i) { +//// if (i == (int) p.type) { +//// points += 3 * res.values[i]; // matches +//// } else { +//// points -= res.values[i]; // does not match +//// } +//// } + +//// int maxIdx = getMaxIdx(res); +//// if (maxIdx == (int) p.type) { +//// ++points; +//// } + +// } + +// } + +// } + +// std::cout << points << std::endl; +// return -points; + +// } + + +// }; + +// K::NumOptAlgoGenetic opt; +// K::NumOptVector vec; +// K::NeuralNetIHO net; +// MyOpt func(practices, net); + +// opt.setElitism(0.025f); +// opt.setPopulationSize(300); +// opt.setMaxIterations(100); +// opt.setMutation(0.10f); +// opt.setValRange(0.5); +// opt.calculateOptimum(func, vec); + + +//// // process every practice +//// for (Practice& p : practices) { + +//// // get the values for the neural-net-input + +//// K::Interpolator interpol = p.getInterpol(); + +//// // process every (positive) occurence within the practice +//// for (uint64_t ts : p.keyGyro) { + +//// std::vector values = getNetworkInput(interpol, ts); +//// K::NeuralNetResultIHO res = net.getOutput(values.data()); +//// debug(p, res); + +//// { +//// std::vector values = getNetworkInput(interpol, ts+500); +//// K::NeuralNetResultIHO res = net.getOutput(values.data()); +//// std::cout << "###"; debug(p, res); +//// } + +//// }getMaxIdx + +//// } + + +// K::Gnuplot gp1; +// K::Gnuplot gp2; + +// K::GnuplotPlot plot1; +// K::GnuplotPlot plot2; + +// K::GnuplotMultiplot plot(2,1); +// plot.add(&plot1); +// plot.add(&plot2); + +// K::GnuplotPlotElementLines line[3]; +// line[0].setColorHex("#ff0000"); line[0].setTitle("x"); +// line[1].setColorHex("#00ff00"); line[1].setTitle("y"); +// line[2].setColorHex("#0000ff"); line[2].setTitle("z"); +// plot1.add(&line[0]); +// plot1.add(&line[1]); +// plot1.add(&line[2]); + +// K::GnuplotPlotElementLines netLines[NUM_OUT]; +// netLines[0].setColorHex("#ff0000"); netLines[0].setTitle("REST"); netLines[0].setLineWidth(2); +// netLines[1].setColorHex("#00ff00"); netLines[1].setTitle("JUMPING_JACK"); netLines[1].setLineWidth(2); +// netLines[2].setColorHex("#0000ff"); netLines[2].setTitle("SITUPS"); netLines[2].setLineWidth(2); +// netLines[3].setColorHex("#ffff00"); netLines[3].setTitle("PUSBACKS"); netLines[3].setLineWidth(2); + +// for (int i = 0; i < NUM_OUT; ++i) { +// plot2.add(&netLines[i]); +// } + +// // process every practice +// for (Practice& p : practices) { + +// // get the values for the neural-net-input + +// K::Interpolator interpol = p.getInterpol(); + +// line[0].clear(); +// line[1].clear(); +// line[2].clear(); + +// for (int i = 0; i < NUM_OUT; ++i) { +// netLines[i].clear(); +// } + +// for (int ms = 0; ms < 20000; ms += 50) { +// SensorGyro s = interpol.get(ms); +// line[0].add(K::GnuplotPoint2(ms, s.x)); +// line[1].add(K::GnuplotPoint2(ms, s.y)); +// line[2].add(K::GnuplotPoint2(ms, s.z)); +// } + +// // process every (positive) occurence within the practice +// for (int ts = 1000; ts < 10000; ts += 50) { + +// std::vector values = getNetworkInput(interpol, ts); +// K::NeuralNetResultIHO res = net.getOutput(values.data()); +// debug(p, res); + +// for (int i = 0; i < NUM_OUT; ++i) { +// netLines[i].add(K::GnuplotPoint2(ts, res.values[i])); +// } + +// gp1 << "set arrow 1 from " << ts-500 << ",-10 to " << ts-500 << ",+10\n"; +// gp1 << "set arrow 2 from " << ts+500 << ",-10 to " << ts+500 << ",+10\n"; +// gp1.draw(plot1); +// gp1.flush(); + +// gp2.draw(plot2); +// gp2.flush(); + +// usleep(1000*33); + + +// } + +// } + + + +//// K::Gnuplot gp; +//// K::GnuplotPlot plot; +//// K::GnuplotPlotElementLines line[3]; +//// line[0].setColorHex("#ff0000"); line[0].setTitle("x"); +//// line[1].setColorHex("#00ff00"); line[1].setTitle("y"); +//// line[2].setColorHex("#0000ff"); line[2].setTitle("z"); + +//// Practice p1 = practices[0]; + +//// auto interpol = p1.getInterpol(); +//// for (int ms = 0; ms < 20000; ms += 50) { +//// SensorGyro s = blur(interpol, ms, 10); +//// line[0].add(K::GnuplotPoint2(ms, s.x)); +//// line[1].add(K::GnuplotPoint2(ms, s.y)); +//// line[2].add(K::GnuplotPoint2(ms, s.z)); +//// } + +//// plot.add(&line[0]); +//// plot.add(&line[1]); +//// plot.add(&line[2]); +//// gp.draw(plot); +//// for (uint64_t ts : p1.keyGyro) { +//// gp << "set arrow from " << ts << ",-10 to " << ts << ",+10\n"; +//// } +//// gp.flush(); + + +// sleep(1000); + +//} diff --git a/workspace/sensors/Recording.h b/workspace/sensors/Recording.h new file mode 100644 index 0000000..d5b3c32 --- /dev/null +++ b/workspace/sensors/Recording.h @@ -0,0 +1,21 @@ +#ifndef RECORDING_H +#define RECORDING_H + +#include "SensorReadings.h" + +#include "SensorMagneticField.h" +#include "SensorAccelerometer.h" +#include "SensorGyro.h" + +/** + * all recorded sensor values within one dataset + */ +struct Recording { + + SensorReadings gyro; + SensorReadings accel; + SensorReadings magField; + +}; + +#endif // RECORDING_H diff --git a/workspace/sensors/SensorAccelerometer.h b/workspace/sensors/SensorAccelerometer.h new file mode 100644 index 0000000..1ece9a9 --- /dev/null +++ b/workspace/sensors/SensorAccelerometer.h @@ -0,0 +1,25 @@ +#ifndef SENSORACCELEROMETER_H +#define SENSORACCELEROMETER_H + +struct SensorAccelerometer { + + float x; + float y; + float z; + + /** empty ctor */ + SensorAccelerometer() : x(0), y(0), z(0) {;} + + /** ctor with values */ + SensorAccelerometer(const float x, const float y, const float z) : x(x), y(y), z(z) {;} + + SensorAccelerometer operator + (const SensorAccelerometer& o) const { + return SensorAccelerometer(x+o.x, y+o.y, z+o.z); + } + SensorAccelerometer operator * (const float v) const { + return SensorAccelerometer(x*v, y*v, z*v); + } + +}; + +#endif // SENSORACCELEROMETER_H diff --git a/workspace/sensors/SensorGyro.h b/workspace/sensors/SensorGyro.h new file mode 100644 index 0000000..0603576 --- /dev/null +++ b/workspace/sensors/SensorGyro.h @@ -0,0 +1,25 @@ +#ifndef SENSORGYRO_H +#define SENSORGYRO_H + +struct SensorGyro { + + float x; + float y; + float z; + + /** empty ctor */ + SensorGyro() : x(0), y(0), z(0) {;} + + /** ctor with values */ + SensorGyro(const float x, const float y, const float z) : x(x), y(y), z(z) {;} + + SensorGyro operator + (const SensorGyro& o) const { + return SensorGyro(x+o.x, y+o.y, z+o.z); + } + SensorGyro operator * (const float v) const { + return SensorGyro(x*v, y*v, z*v); + } + +}; + +#endif // SENSORGYRO_H diff --git a/workspace/sensors/SensorMagneticField.h b/workspace/sensors/SensorMagneticField.h new file mode 100644 index 0000000..952c0c2 --- /dev/null +++ b/workspace/sensors/SensorMagneticField.h @@ -0,0 +1,25 @@ +#ifndef SENSORMAGNETICFIELD_H +#define SENSORMAGNETICFIELD_H + +struct SensorMagneticField { + + float x; + float y; + float z; + + /** empty ctor */ + SensorMagneticField() : x(0), y(0), z(0) {;} + + /** ctor with values */ + SensorMagneticField(const float x, const float y, const float z) : x(x), y(y), z(z) {;} + + SensorMagneticField operator + (const SensorMagneticField& o) const { + return SensorMagneticField(x+o.x, y+o.y, z+o.z); + } + SensorMagneticField operator * (const float v) const { + return SensorMagneticField(x*v, y*v, z*v); + } + +}; + +#endif // SENSORMAGNETICFIELD_H diff --git a/workspace/sensors/SensorReader.h b/workspace/sensors/SensorReader.h new file mode 100644 index 0000000..e0cec95 --- /dev/null +++ b/workspace/sensors/SensorReader.h @@ -0,0 +1,129 @@ +#ifndef SENSORREADER_H +#define SENSORREADER_H + +#include +#include +#include +#include +#include + +#include "Recording.h" +#include "Sensors.h" + +/** + * parse all sensor values from HandyGames data-files + */ +class SensorReader { + +private: + + static constexpr int bufSize = 4096; + +public: + + /** parse all values within the given file */ + static Recording read(const std::string& file) { + + Recording rec; + Sensors curSensor = Sensors::UNKNOWN; + char buf[bufSize]; + + // check + std::ifstream f(file.c_str()); + if (!f.good()) {throw "error!";} + + // parse each line + while(!f.bad() && !f.eof()) { + + // read the next line + f.getline(buf, bufSize); + std::string line(buf); + + // new sensor section? -> switch the current sensor + if (startsWith(line, "// [SENSOR] ")) { + std::string tmp = line.substr(19); + int idx = indexOf(tmp, ";"); + curSensor = getSensor(tmp.substr(0, idx)); + } + + // skip empty lines + else if (line.empty()) {;} + + // parse sensor values + else { + switch (curSensor) { + case Sensors::TYPE_ACCELEROMETER: parseAccel(rec, line); break; + case Sensors::TYPE_GYROSCOPE: parseGyro(rec, line); break; + case Sensors::TYPE_MAGNETIC_FIELD: parseMagField(rec, line); break; + default: break; + } + } + + + } + + // done + return rec; + + } + + static void parseAccel(Recording& rec, const std::string& line) { + const std::vector values = split(line); + rec.accel.add(getTS(values[0]), SensorAccelerometer(getFloat(values[1]), getFloat(values[2]), getFloat(values[3]))); + } + + static void parseGyro(Recording& rec, const std::string& line) { + const std::vector values = split(line); + rec.gyro.add(getTS(values[0]), SensorGyro(getFloat(values[1]), getFloat(values[2]), getFloat(values[3]))); + } + + static void parseMagField(Recording& rec, const std::string& line) { + const std::vector values = split(line); + rec.magField.add(getTS(values[0]), SensorMagneticField(getFloat(values[1]), getFloat(values[2]), getFloat(values[3]))); + } + + + + /** get the timestamp for the given string-value */ + static uint64_t getTS(const std::string& s) { + return std::stoul(s) / 1000 / 1000; + } + + /** convert the given string to a float value */ + static float getFloat(const std::string& s) { + return std::stof(s); + } + + /** does the given haystack start with the given needle? */ + static bool startsWith(const std::string& haystack, const std::string& needle) { + if (needle.length() > haystack.length()) {return false;} + return memcmp(haystack.data(), needle.data(), needle.length()) == 0; + } + + /** position of the first occurence of needle within haystack */ + static int indexOf(const std::string& haystack, const std::string& needle, const int start = 0) { + std::string::size_type index = haystack.find(needle, start); + return (index == std::string::npos ) ? (-1) : (index); + } + + static std::vector split(const std::string& str) { + std::vector list; + int last = 0; + while(true) { + const int next = indexOf(str, ",", last); + if (next == -1) { + std::string sub = str.substr(last, str.length()-last-2); + list.push_back(sub); + break; + } else { + std::string sub = str.substr(last, next-last); + list.push_back(sub); + last = next+2; + } + } + return list; + } + +}; + +#endif // SENSORREADER_H diff --git a/workspace/sensors/SensorReadings.h b/workspace/sensors/SensorReadings.h new file mode 100644 index 0000000..37dcfb3 --- /dev/null +++ b/workspace/sensors/SensorReadings.h @@ -0,0 +1,29 @@ +#ifndef SENSORREADINGS_H +#define SENSORREADINGS_H + +#include + +template class SensorReadings { + + /** combine sensor-values with a timestamp */ + struct TimedEntry { + uint64_t ts; + T val; + TimedEntry(const uint64_t ts, const T& val) : ts(ts), val(val) {;} + }; + +public: + + /** all readings (with timestamp) for one sensor */ + std::vector values; + +public: + + /** add a new sensor-reading with timestamp */ + void add(const uint64_t ts, const T& val) { + values.push_back(TimedEntry(ts, val)); + } + +}; + +#endif // SENSORREADINGS_H diff --git a/workspace/sensors/Sensors.h b/workspace/sensors/Sensors.h new file mode 100644 index 0000000..0fbe071 --- /dev/null +++ b/workspace/sensors/Sensors.h @@ -0,0 +1,19 @@ +#ifndef SENSORS_H +#define SENSORS_H + +enum class Sensors { + UNKNOWN, + TYPE_ACCELEROMETER, + TYPE_GYROSCOPE, + TYPE_MAGNETIC_FIELD, +}; + +/** convert string to sensor-enum */ +static Sensors getSensor(const std::string& s) { + if ("TYPE_MAGNETIC_FIELD" == s) {return Sensors::TYPE_MAGNETIC_FIELD;} + else if ("TYPE_ACCELEROMETER" == s) {return Sensors::TYPE_ACCELEROMETER;} + else if ("TYPE_GYROSCOPE" == s) {return Sensors::TYPE_GYROSCOPE;} + else {return Sensors::UNKNOWN;} +} + +#endif // SENSORS_H