diff --git a/.gitignore b/.gitignore
index eecacf5486e21fa012af192d1dcae6574a9d4cfe..505aa0582bff911578c2b61139bddc917efcabbf 100644
--- a/.gitignore
+++ b/.gitignore
@@ -60,3 +60,4 @@ scenarios/*/*/ttyACM*.log
scenarios/*/*/config/datapath.cfg
*.icegrid.xml
+data/RobotAPI/logs
diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index bdfd4460bdbaddacd22a1b59de3f4746741d43fa..0537f558b61fc877967992b3a2ca57e1eb959362 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -57,6 +57,14 @@
propertyName="IMUObserverName"
propertyIsOptional="true"
propertyDefaultValue="InertialMeasurementUnitObserver" />
+ <Proxy include="RobotAPI/interface/units/OrientedTactileSensorUnit.h"
+ humanName="Oriented Tactile Sensor Unit Observer"
+ typeName="OrientedTactileSensorUnitObserverInterfacePrx"
+ memberName="orientedTactileSensorUnitObserver"
+ getterName="getOrientedTactileSensorUnitObserver"
+ propertyName="OrientedTactileSensorUnitObserverName"
+ propertyIsOptional="true"
+ propertyDefaultValue="OrientedTactileSensorUnitObserver" />
<Proxy include="RobotAPI/interface/units/ForceTorqueUnit.h"
humanName="Force Torque Unit Observer"
typeName="ForceTorqueUnitObserverInterfacePrx"
diff --git a/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx b/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
index 443162d742ef69c2d3d6bedc728ac05494254fc2..23c06bee20258339aa1284f1d0c6d9ba2bef21ff 100644
--- a/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
+++ b/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="utf-8"?>
-<scenario name="OrientedTactileSensor" lastChange="2017-03-06.17:30:06" creation="2017-02-27.01:48:55 PM" globalConfigName="./config/global.cfg" package="RobotAPI">
- <application name="OrientedTactileSensorUnitApp" instance="" package="RobotAPI"/>
- <application name="OrientedTactileSensorUnitObserverApp" instance="" package="RobotAPI"/>
+<scenario name="OrientedTactileSensor" creation="2017-02-27.01:48:55 PM" globalConfigName="./config/global.cfg" package="RobotAPI">
+ <application name="OrientedTactileSensorUnitApp" instance="" package="RobotAPI" enabled="true"/>
+ <application name="OrientedTactileSensorUnitObserverApp" instance="" package="RobotAPI" enabled="true"/>
</scenario>
diff --git a/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg b/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
index 1ec6764f36d51be5c6b8501bb6ef5e5046114c79..01ddb0e7eb3ba7c1413f097f884ed698ec30bd0c 100644
--- a/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
+++ b/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
@@ -79,7 +79,15 @@
# - Default: 65524 3 12 65534 65534 1 1208 119 58726 1000 943
# - Case sensitivity: no
# - Required: no
-ArmarX.OrientedTactileSensorUnit.CalibrationData = 65524 3 12 65534 65534 1 1208 119 58726 1000 943
+ArmarX.OrientedTactileSensorUnit.CalibrationData = 65535 65535 65535 65535 65535 65535 65535 65535 65535 65535 65535
+
+
+# ArmarX.OrientedTactileSensorUnit.DebugDrawerTopicName: Name of the debug drawer topic that should be used
+# Attributes:
+# - Default: DebugDrawerUpdates
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.OrientedTactileSensorUnit.DebugDrawerTopicName = DebugDrawerUpdates
# ArmarX.OrientedTactileSensorUnit.EnableProfiling: enable profiler which is used for logging performance events
@@ -143,7 +151,7 @@ ArmarX.OrientedTactileSensorUnit.SerialInterfaceDevice = /dev/ttyACM0
# - Default: OrientedTactileSensorValues
# - Case sensitivity: no
# - Required: no
-# ArmarX.OrientedTactileSensorUnit.TopicName = OrientedTactileSensorValues
+ArmarX.OrientedTactileSensorUnit.TopicName = OrientedTactileSensorValues
# ArmarX.OrientedTactileSensorUnit.calibrateSensor:
@@ -154,6 +162,14 @@ ArmarX.OrientedTactileSensorUnit.SerialInterfaceDevice = /dev/ttyACM0
ArmarX.OrientedTactileSensorUnit.calibrateSensor = 0
+# ArmarX.OrientedTactileSensorUnit.logData: Custom Property
+# Attributes:
+# - Default: ::NOT_DEFINED::
+# - Case sensitivity: no
+# - Required: no
+ArmarX.OrientedTactileSensorUnit.logData = 1
+
+
# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
# Attributes:
# - Default: 1
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.cpp b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
index 10e5e49547a834e2b6577333eb63433ef0d38a5c..bdf8dc94e2068fb373f5f5984604d262bb78d1a1 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
@@ -296,14 +296,14 @@ void KinematicUnitObserver::nameValueMapToDataFields(const std::string& channelN
boost::unordered_map< ::std::string, ::armarx::VariantBasePtr> map;
if (timestamp < 0)
{
- for (const auto & it : nameValueMap)
+ for (const auto& it : nameValueMap)
{
map[it.first] = new Variant(it.second);
}
}
else
{
- for (const auto & it : nameValueMap)
+ for (const auto& it : nameValueMap)
{
map[it.first] = new TimedVariant(new Variant(it.second), IceUtil::Time::microSeconds(timestamp));
}
diff --git a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
index 906e90f79bb7593b36e23e985357c78076fc2512..7762139ad1d7ac849a06f111ecccd5982fe8bf7e 100644
--- a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
+++ b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
@@ -74,7 +74,7 @@ void OrientedTactileSensorUnitObserver::reportSensorValues(int id, float pressur
offerOrUpdateDataField(channelName, "pressure", Variant(pressure), "current pressure");
QuaternionPtr orientationQuaternion = new Quaternion(qw, qx, qy, qz);
- offerOrUpdateDataField(channelName, "orientation", orientationQuaternion, "current oriantation");
+ offerOrUpdateDataField(channelName, "orientation", orientationQuaternion, "current orientation");
offerOrUpdateDataField(channelName, "rotationRate", Variant(rotationRate), "current rotationRate");
offerOrUpdateDataField(channelName, "pressureRate", Variant(pressureRate), "current pressureRate");
offerOrUpdateDataField(channelName, "accelerationRate", Variant(accelerationRate), "current accelerationRate");
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/CMakeLists.txt b/source/RobotAPI/drivers/OrientedTactileSensor/CMakeLists.txt
index eefa4ca3137b61c4c20707c2592e5096437a755e..e362b40cd817888054e4c6f5b2716373f44feeac 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/CMakeLists.txt
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/CMakeLists.txt
@@ -13,7 +13,7 @@ set(LIB_NAME OrientedTactileSensor)
-set(LIBS RobotAPIUnits ArmarXCoreObservers ArmarXCoreEigen3Variants)
+set(LIBS SimpleJsonLogger RobotAPIUnits ArmarXCoreObservers ArmarXCoreEigen3Variants)
set(LIB_FILES
OrientedTactileSensorUnit.cpp
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
index 2820435ebabc2a9853c838518430fc9e13f82a22..d1b874e27e70b15e10522a04039359ed0aeba32b 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
@@ -1,10 +1,12 @@
#include "OrientedTactileSensorUnit.h"
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <termios.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <math.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
+#include <RobotAPI/libraries/core/Pose.h>
using namespace armarx;
@@ -14,22 +16,42 @@ OrientedTactileSensorUnit::OrientedTactileSensorUnit()
sampleIndexRotation = 0;
sampleIndexPressure = 0;
sampleIndexAcceleration = 0;
+ sampleIndexPressureRate = 0;
}
void OrientedTactileSensorUnit::onInitComponent()
{
- maxSamplesRotation = getProperty<std::size_t>("SamplesRotation").getValue();
- maxSamplesPressure = getProperty<std::size_t>("SamplesPressure").getValue();
- maxSamplesAcceleration = getProperty<std::size_t>("SamplesAcceleration").getValue();
+ //logger part
+ if (getProperty<bool>("logData").getValue())
+ {
+ IceUtil::Time now = IceUtil::Time::now();
+ time_t timer = now.toSeconds();
+ struct tm* ts;
+ char buffer[80];
+ ts = localtime(&timer);
+ strftime(buffer, 80, "%Y-%m-%d-%H-%M-%S", ts);
+ std::string packageName = "RobotAPI";
+ armarx::CMakePackageFinder finder(packageName);
+ std::string dataDir = finder.getDataDir() + "/" + packageName + "/logs/";
+ std::string filename = dataDir + buffer + std::string("_data") + ".json";
+ //ARMARX_IMPORTANT << filename;
+
+ logger.reset(new SimpleJsonLogger(filename, true));
+ prefix = std::string(buffer);
+ }
+ maxSamplesRotation = stoi(getProperty<std::string>("SamplesRotation").getValue());
+ maxSamplesPressure = stoi(getProperty<std::string>("SamplesPressure").getValue());
+ maxSamplesAcceleration = stoi(getProperty<std::string>("SamplesAcceleration").getValue());
std::string topicName = getProperty<std::string>("TopicName").getValue();
offeringTopic(topicName);
//open serial port
std::string portname = getProperty<std::string>("SerialInterfaceDevice").getValue();
- arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::in);
+ arduinoIn.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::in);
+ arduinoOut.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::out);
- fd = open(portname.c_str(), O_RDONLY | O_NOCTTY);
+ fd = open(portname.c_str(), O_RDWR | O_NOCTTY);
struct termios toptions;
/* Get currently set options for the tty */
@@ -55,7 +77,7 @@ void OrientedTactileSensorUnit::onInitComponent()
ARMARX_INFO << "opening device " << getProperty<std::string>("SerialInterfaceDevice").getValue();
- if (!arduino.is_open())
+ if (!arduinoIn.is_open())
{
throw LocalException("Cannot open Arduino on ") << getProperty<std::string>("SerialInterfaceDevice").getValue();
@@ -66,6 +88,7 @@ void OrientedTactileSensorUnit::onInitComponent()
//wait for the Arduino to reboot
usleep(4000000);
std::string arduinoLine;
+
//wait for the IMU to be calibrated or load calibration
ARMARX_INFO << "waiting for IMU calibration - this can take some time";
if (getProperty<bool>("calibrateSensor").getValue())
@@ -74,22 +97,18 @@ void OrientedTactileSensorUnit::onInitComponent()
while (arduinoLine.find("mode") == std::string::npos)
{
- getline(arduino, arduinoLine, '\n');
+ getline(arduinoIn, arduinoLine, '\n');
}
- arduino.close();
- arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::out);
- arduino << "calibrate";
- arduino.flush();
- arduino.close();
+ arduinoOut << "calibrate";
+ arduinoOut.flush();
- arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::in);
while (arduinoLine.find("Calibration Sucessfull") == std::string::npos)
{
- getline(arduino, arduinoLine, '\n');
+ getline(arduinoIn, arduinoLine, '\n');
ARMARX_INFO << arduinoLine;
}
- getline(arduino, arduinoLine, '\n');
+ getline(arduinoIn, arduinoLine, '\n');
if (getCalibrationValues(arduinoLine))
{
ARMARX_IMPORTANT << "calibrated sensor";
@@ -102,6 +121,14 @@ void OrientedTactileSensorUnit::onInitComponent()
if (loadCalibration())
{
ARMARX_IMPORTANT << "loaded calibration";
+ first = true;
+ /*std::string line;
+ getline(arduinoIn, line, '\n');
+ ARMARX_IMPORTANT<<line;
+ SensorData dataInit = getValues(line.c_str());
+ ARMARX_IMPORTANT<<dataInit.qw<<" "dataInit.qx<<" "<<dataInit.qy<<" "<<dataInit.qz;
+ Eigen::Quaternionf initialOrientation = Eigen::Quaternionf(dataInit.qw, dataInit.qx, dataInit.qy, dataInit.qz);
+ inverseOrientation = initialOrientation.inverse();*/
}
}
readTask = new RunningTask<OrientedTactileSensorUnit>(this, &OrientedTactileSensorUnit::run);
@@ -111,6 +138,7 @@ void OrientedTactileSensorUnit::onInitComponent()
void OrientedTactileSensorUnit::onConnectComponent()
{
+ debugDrawerTopic = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopicName").getValue());
std::string topicName = getProperty<std::string>("TopicName").getValue();
topicPrx = getTopic<OrientedTactileSensorUnitListenerPrx>(topicName);
}
@@ -126,7 +154,7 @@ void OrientedTactileSensorUnit::run()
while (readTask->isRunning())
{
std::string line;
- getline(arduino, line, '\n');
+ getline(arduinoIn, line, '\n');
SensorData data = getValues(line.c_str());
IceUtil::Time now = IceUtil::Time::now();
TimestampVariantPtr nowTimestamp = new TimestampVariant(now);
@@ -140,6 +168,7 @@ void OrientedTactileSensorUnit::run()
RotationRate sampleRotation;
sampleRotation.timestamp = now;
sampleRotation.orientation = Eigen::Quaternionf(data.qw, data.qx, data.qy, data.qz);
+ //sampleRotation.orientation = sampleRotation.orientation * inverseOrientation;
if (samplesRotation.size() < maxSamplesRotation)
{
samplesRotation.push_back(sampleRotation);
@@ -153,10 +182,10 @@ void OrientedTactileSensorUnit::run()
oldsampleRotation.timestamp = samplesRotation.at(sampleIndexRotation).timestamp;
oldsampleRotation.orientation = samplesRotation.at(sampleIndexRotation).orientation;
Eigen::AngleAxisf aa(sampleRotation.orientation * oldsampleRotation.orientation.inverse());
+ //ARMARX_IMPORTANT << "aa: " << aa.axis() << " " << aa.angle();
rotationRate = aa.angle() / (sampleRotation.timestamp - oldsampleRotation.timestamp).toSecondsDouble();
}
}
-
//compute pressureRate
float pressureRate = 0;
PressureRate samplePressure;
@@ -194,16 +223,93 @@ void OrientedTactileSensorUnit::run()
oldsampleAcceleration.rotationRate = samplesAcceleration.at(sampleIndexAcceleration).rotationRate;
accelerationRate = (sampleAcceleration.rotationRate - oldsampleAcceleration.rotationRate) / (sampleAcceleration.timestamp - oldsampleAcceleration.timestamp).toSecondsDouble();
}
- if ((std::abs(pressureRate) > 50))
+ if (pressureRates.size() < maxSamplesPressure)
+ {
+ pressureRates.push_back(pressureRate);
+ }
+ else
+ {
+ pressureRates[sampleIndexPressureRate] = pressureRate;
+ sampleIndexPressureRate = (sampleIndexPressureRate + 1) % maxSamplesPressure;
+ }
+ if (pressureRate > 50)
{
ARMARX_IMPORTANT << "contact";
}
+ Eigen::Quaternionf orientationQuaternion = Eigen::Quaternionf(data.qw, data.qx, data.qy, data.qz);
+ if (getProperty<bool>("logData").getValue())
+ {
+
+ if (i < 50)
+ {
+ inverseOrientation = orientationQuaternion.inverse();
+ i++;
+ }
+ Eigen::Matrix3f quatMatrix = orientationQuaternion.toRotationMatrix();
+ Eigen::Matrix4f quat4Matrix = Eigen::Matrix4f::Identity();
+
+ quat4Matrix.block(0, 0, 3, 3) = quatMatrix;
+
+ Eigen::Vector3f linearAcceleration(data.accelx, data.accely, data.accelz);
+ SimpleJsonLoggerEntry e;
+ e.AddTimestamp();
+ e.Add("Pressure", data.pressure);
+ e.Add("PressureRate", pressureRate);
+ e.Add("RotationRate", rotationRate);
+ e.AddAsArr("Orientation", quat4Matrix);
+ e.AddAsArr("Linear Acceleration", linearAcceleration);
+ logger->log(e);
+ }
+ /*Eigen::Matrix3f rotZ;
+ rotZ(0, 0) = 0;
+ rotZ(0, 1) = 1;
+ rotZ(0, 2) = 0;
+ rotZ(1, 0) = -1;
+ rotZ(1, 1) = 0;
+ rotZ(1, 2) = 0;
+ rotZ(2, 0) = 0;
+ rotZ(2, 1) = 0;
+ rotZ(2, 2) = 1;
+ Eigen::Matrix3f rotX;
+ rotX(0, 0) = 1;
+ rotX(0, 1) = 0;
+ rotX(0, 2) = 0;
+ rotX(1, 0) = 0;
+ rotX(1, 1) = -1;
+ rotX(1, 2) = 0;
+ rotX(2, 0) = 0;
+ rotX(2, 1) = 0;
+ rotX(2, 2) = -1;*/
+ Eigen::Matrix3f rotY;
+ rotY(0, 0) = 0;
+ rotY(0, 1) = 0;
+ rotY(0, 2) = 1;
+ rotY(1, 0) = 0;
+ rotY(1, 1) = 1;
+ rotY(1, 2) = 0;
+ rotY(2, 0) = -1;
+ rotY(2, 1) = 0;
+ rotY(2, 2) = 0;
+ Eigen::Matrix3f rawOrientation = orientationQuaternion.toRotationMatrix();
+
+ PosePtr pose = new Pose(rawOrientation, Eigen::Vector3f(100.0, 200.0, 0.0));
+ if (debugDrawerTopic)
+ {
+ debugDrawerTopic->setPoseVisu("debugdrawerlayer", "pose", pose);
+ }
+ Eigen::Quaternionf quaternion(rawOrientation);
+ data.qw = quaternion.w();
+ data.qx = quaternion.x();
+ data.qy = quaternion.y();
+ data.qz = quaternion.z();
+ ARMARX_IMPORTANT << "or " << orientationQuaternion.w() << " " << orientationQuaternion.x() << " " << orientationQuaternion.y() << " " << orientationQuaternion.z();
if (topicPrx)
{
topicPrx->reportSensorValues(data.id, data.pressure, data.qw, data.qx, data.qy, data.qz, pressureRate, rotationRate, accelerationRate, data.accelx, data.accely, data.accelz, nowTimestamp);
}
}
+
}
// get imu values from incoming string
@@ -231,32 +337,24 @@ bool OrientedTactileSensorUnit::loadCalibration()
std::string arduinoLine;
while (arduinoLine.find("mode") == std::string::npos)
{
- getline(arduino, arduinoLine, '\n');
+ getline(arduinoIn, arduinoLine, '\n');
+ ARMARX_INFO << arduinoIn;
}
- arduino.close();
-
- arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::out);
- arduino << "load";
- arduino.flush();
- arduino.close();
-
- arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::in);
+ arduinoOut << "load";
+ arduinoOut.flush();
while (arduinoLine.find("calibration data") == std::string::npos)
{
- getline(arduino, arduinoLine, '\n');
+ getline(arduinoIn, arduinoLine, '\n');
+ ARMARX_INFO << arduinoIn;
}
- arduino.close();
-
- arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::out);
- arduino << calibrationStream;
- arduino.flush();
- arduino.close();
- arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::in);
+ arduinoOut << calibrationStream;
+ arduinoOut.flush();
while (arduinoLine.find("Calibration Sucessfull") == std::string::npos)
{
- getline(arduino, arduinoLine, '\n');
+ getline(arduinoIn, arduinoLine, '\n');
+ //ARMARX_INFO << arduinoIn;
}
return true;
}
@@ -283,5 +381,3 @@ bool OrientedTactileSensorUnit::getCalibrationValues(std::string line)
ARMARX_IMPORTANT << "calibration data: " << calibrationStream ;
return true;
}
-
-
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
index c518ac3f186d7e0662a4a7d527beb0cb4853abbf..5bacc20d35be89a959a2cc379eb546c19f82219a 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
@@ -7,10 +7,13 @@
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <netinet/in.h>
+#include <iostream>
#include <fstream>
#include <stdio.h>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <Eigen/Dense>
+#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
namespace armarx
{
@@ -36,27 +39,31 @@ namespace armarx
"65524 3 12 65534 65534 1 1208 119 58726 1000 943 ",
"Sensor Register Data to calibrate the sensor");
- defineOptionalProperty<std::size_t>(
+ defineOptionalProperty<std::string>(
"SamplesRotation",
- 20,
+ "20",
"number of orientation values to differentiate");
- defineOptionalProperty<std::size_t>(
+ defineOptionalProperty<std::string>(
"SamplesPressure",
- 10,
+ "10",
"number of pressure values to differentiate");
- defineOptionalProperty<std::size_t>(
+ defineOptionalProperty<std::string>(
"SamplesAcceleration",
- 20,
+ "20",
"number of pressure values to differentiate");
+ defineOptionalProperty<bool>(
+ "logData",
+ "false",
+ "log data from sensor");
defineOptionalProperty<bool>(
"calibrateSensor",
"false"
"Set true to calibrate the sensor and get calibration data and false to use existent calibration data");
+ defineOptionalProperty<std::string>("DebugDrawerTopicName", "DebugDrawerUpdates", "Name of the debug drawer topic that should be used");
}
-
};
/**
@@ -119,7 +126,8 @@ namespace armarx
virtual PropertyDefinitionsPtr createPropertyDefinitions();
private:
- std::fstream arduino;
+ std::ifstream arduinoIn;
+ std::ofstream arduinoOut;
RunningTask<OrientedTactileSensorUnit>::pointer_type readTask;
OrientedTactileSensorUnitListenerPrx topicPrx;
OrientedTactileSensorUnitInterfacePrx interfacePrx;
@@ -131,15 +139,26 @@ namespace armarx
int fd;
CalibrationData calibration;
+ Eigen::Quaternionf inverseOrientation;
std::vector<RotationRate> samplesRotation;
std::vector<PressureRate> samplesPressure;
std::vector<AccelerationRate> samplesAcceleration;
- std::size_t maxSamplesRotation;
- std::size_t sampleIndexRotation;
- std::size_t maxSamplesPressure;
- std::size_t sampleIndexPressure;
- std::size_t maxSamplesAcceleration;
- std::size_t sampleIndexAcceleration;
+ std::vector<float> pressureRates;
+ //Eigen::AngleAxisf aa;
+ int maxSamplesRotation;
+ int sampleIndexRotation;
+ int maxSamplesPressure;
+ int sampleIndexPressure;
+ int maxSamplesAcceleration;
+ int sampleIndexAcceleration;
+ int sampleIndexPressureRate;
+ float sumPressureRates;
+ Eigen::Matrix3f sumOrientation;
+ bool first;
+ int i = 0;
+ DebugDrawerInterfacePrx debugDrawerTopic;
+ SimpleJsonLoggerPtr logger;
+ std::string prefix;
};
}
#endif // SENSORPACKAGEUNIT_H
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h.orig b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h.orig
new file mode 100644
index 0000000000000000000000000000000000000000..a10822f8e98f1bcbb85d4ed3fd76c2a5810497b6
--- /dev/null
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h.orig
@@ -0,0 +1,169 @@
+#ifndef SENSORPACKAGEUNIT_H
+#define SENSORPACKAGEUNIT_H
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/units/UnitInterface.h>
+#include <RobotAPI/interface/units/OrientedTactileSensorUnit.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
+#include <netinet/in.h>
+#include <iostream>
+#include <fstream>
+#include <stdio.h>
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <Eigen/Dense>
+#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
+
+namespace armarx
+{
+ class OrientedTactileSensorUnitPropertyDefinitions:
+ public ComponentPropertyDefinitions
+ {
+ public:
+ OrientedTactileSensorUnitPropertyDefinitions(std::string prefix):
+ ComponentPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::string>(
+ "SerialInterfaceDevice",
+ "/dev/ttyACM0",
+ "The serial device the arduino is connected to.");
+
+ defineOptionalProperty<std::string>(
+ "TopicName",
+ "OrientedTactileSensorValues",
+ "Name of the topic on which the sensor values are provided");
+
+ defineOptionalProperty<std::string>(
+ "CalibrationData",
+ "65524 3 12 65534 65534 1 1208 119 58726 1000 943 ",
+ "Sensor Register Data to calibrate the sensor");
+
+ defineOptionalProperty<std::size_t>(
+ "SamplesRotation",
+ 20,
+ "number of orientation values to differentiate");
+
+ defineOptionalProperty<std::size_t>(
+ "SamplesPressure",
+ 10,
+ "number of pressure values to differentiate");
+
+ defineOptionalProperty<std::size_t>(
+ "SamplesAcceleration",
+ 20,
+ "number of pressure values to differentiate");
+
+ defineOptionalProperty<bool>(
+ "logData",
+ "false",
+ "log data from sensor");
+ defineOptionalProperty<bool>(
+ "calibrateSensor",
+ "false"
+ "Set true to calibrate the sensor and get calibration data and false to use existent calibration data");
+ }
+
+ };
+
+ /**
+ * @class OrientedTactileSensorUnit
+ * @brief ArmarX wrapper for an arduino due with one BNO055 IMU and one BMP280 pressure sensor
+ *
+ */
+ class OrientedTactileSensorUnit:
+ virtual public armarx::Component
+ //TODO: needs interface to send calibration data
+ {
+ public:
+ OrientedTactileSensorUnit();
+
+ virtual std::string getDefaultName() const
+ {
+ return "OrientedTactileSensorUnit";
+ }
+
+ struct SensorData
+ {
+ int id;
+ float pressure;
+ float qw, qx, qy, qz;
+ float accelx, accely, accelz;
+ };
+
+ struct CalibrationData
+ {
+ int accel_offset_x, accel_offset_y, accel_offset_z, gyro_offset_x, gyro_offset_y, gyro_offset_z, mag_offset_x, mag_offset_y, mag_offset_z, accel_radius, mag_radius;
+ };
+
+ struct PressureRate
+ {
+ IceUtil::Time timestamp;
+ float pressure;
+ };
+
+ struct RotationRate
+ {
+ IceUtil::Time timestamp;
+ Eigen::Quaternionf orientation;
+ };
+
+ struct AccelerationRate
+ {
+ IceUtil::Time timestamp;
+ float rotationRate;
+ };
+ struct LinAccRate
+ {
+ IceUtil::Time timestamp;
+ float accx, accy, accz;
+ };
+
+ protected:
+ virtual void onInitComponent();
+ virtual void onConnectComponent();
+
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ private:
+ std::ifstream arduinoIn;
+ std::ofstream arduinoOut;
+ RunningTask<OrientedTactileSensorUnit>::pointer_type readTask;
+ OrientedTactileSensorUnitListenerPrx topicPrx;
+ OrientedTactileSensorUnitInterfacePrx interfacePrx;
+
+ void run();
+ SensorData getValues(std::string line);
+ bool getCalibrationValues(std::string line);
+ bool loadCalibration();
+ int fd;
+ CalibrationData calibration;
+
+ Eigen::Quaternionf inverseOrientation;
+ std::vector<RotationRate> samplesRotation;
+ std::vector<PressureRate> samplesPressure;
+ std::vector<AccelerationRate> samplesAcceleration;
+<<<<<<< HEAD
+ std::vector<float> pressureRates;
+ int maxSamplesRotation;
+ int sampleIndexRotation;
+ int maxSamplesPressure;
+ int sampleIndexPressure;
+ int maxSamplesAcceleration;
+ int sampleIndexAcceleration;
+ int sampleIndexPressureRate;
+ float sumPressureRates;
+ bool first;
+ int i = 0;
+ SimpleJsonLoggerPtr logger;
+ std::string prefix;
+=======
+ std::size_t maxSamplesRotation;
+ std::size_t sampleIndexRotation;
+ std::size_t maxSamplesPressure;
+ std::size_t sampleIndexPressure;
+ std::size_t maxSamplesAcceleration;
+ std::size_t sampleIndexAcceleration;
+>>>>>>> master
+ };
+}
+#endif // SENSORPACKAGEUNIT_H
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 10a98da381b0b0b4da90b92fdecfad188f08a1e7..81101dffb41fd0532989a2354bd8b24183234326 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -1,4 +1,4 @@
add_subdirectory(core)
add_subdirectory(widgets)
-
+add_subdirectory(SimpleJsonLogger)
add_subdirectory(RobotAPIVariantWidget)
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/CMakeLists.txt b/source/RobotAPI/libraries/SimpleJsonLogger/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8c3a421f764e62f3e71b2db92bd47d2874ca78ec
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/CMakeLists.txt
@@ -0,0 +1,44 @@
+armarx_component_set_name("SimpleJsonLogger")
+armarx_set_target("Library: SimpleJsonLogger")
+
+set(LIB_NAME SimpleJsonLogger)
+
+find_package(Eigen3 QUIET)
+find_package(Simox QUIET)
+#find_package(HapticExplorationSimulation)
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+#armarx_build_if(HapticExplorationSimulation_FOUND "HapticExplorationSimulation not available")
+
+
+if (Eigen3_FOUND AND Simox_FOUND)
+ include_directories(
+ ${Eigen3_INCLUDE_DIR}
+ ${Simox_INCLUDE_DIRS}
+ #${HapticExplorationSimulation_INCLUDE_DIRS}
+ )
+endif()
+
+set(LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ StructuralJson
+)
+
+set(LIB_FILES
+SimpleJsonLogger.cpp
+SimpleJsonLoggerEntry.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(LIB_HEADERS
+SimpleJsonLogger.h
+SimpleJsonLoggerEntry.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+# add unit tests
+add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.cpp b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..696acf03ebb7ae0a3940937f5f84deb95e174cc5
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.cpp
@@ -0,0 +1,66 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "SimpleJsonLogger.h"
+
+using namespace armarx;
+
+SimpleJsonLogger::SimpleJsonLogger(const std::string& filename, bool append)
+ : autoflush(true)
+{
+ file.open(filename.c_str(), append ? std::ios_base::app : std::ios_base::out | std::ios_base::trunc);
+}
+
+void SimpleJsonLogger::log(JsonDataPtr entry)
+{
+ file << entry->toJsonString() << "\n";
+ if (autoflush)
+ {
+ file.flush();
+ }
+}
+
+void SimpleJsonLogger::log(const SimpleJsonLoggerEntry& entry)
+{
+ log(entry.obj);
+}
+
+void SimpleJsonLogger::log(SimpleJsonLoggerEntryPtr entry)
+{
+ log(entry->obj);
+}
+
+void SimpleJsonLogger::close()
+{
+ file.flush();
+ file.close();
+}
+
+SimpleJsonLogger::~SimpleJsonLogger()
+{
+ if (file.is_open())
+ {
+ file.close();
+ }
+}
+
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h
new file mode 100644
index 0000000000000000000000000000000000000000..857ae73ce15b83fe440f87b95ce68e6e608421d1
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h
@@ -0,0 +1,61 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef armarx_SimpleJsonLogger_SimpleJsonLogger
+#define armarx_SimpleJsonLogger_SimpleJsonLogger
+
+#include "SimpleJsonLoggerEntry.h"
+
+#include <boost/shared_ptr.hpp>
+
+#include <iostream>
+#include <fstream>
+
+#include <ArmarXGui/libraries/StructuralJson/JsonArray.h>
+#include <ArmarXGui/libraries/StructuralJson/JsonData.h>
+#include <ArmarXGui/libraries/StructuralJson/JsonObject.h>
+
+#include <Eigen/Dense>
+
+namespace armarx
+{
+ class SimpleJsonLogger;
+ typedef boost::shared_ptr<SimpleJsonLogger> SimpleJsonLoggerPtr;
+
+ class SimpleJsonLogger
+ {
+ public:
+ SimpleJsonLogger(const std::string& filename, bool append);
+ void log(JsonDataPtr entry);
+ void log(const SimpleJsonLoggerEntry& entry);
+ void log(SimpleJsonLoggerEntryPtr entry);
+ void close();
+ ~SimpleJsonLogger();
+
+ private:
+ bool autoflush;
+ std::ofstream file;
+ };
+}
+
+#endif // armarx_SimpleJsonLogger_SimpleJsonLogger
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bd5c58bee5aec1229ab8e33fc707495deb6dbcc3
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp
@@ -0,0 +1,118 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "SimpleJsonLoggerEntry.h"
+#include <IceUtil/Time.h>
+
+using namespace armarx;
+
+SimpleJsonLoggerEntry::SimpleJsonLoggerEntry()
+ : obj(new JsonObject)
+{
+}
+
+void SimpleJsonLoggerEntry::AddAsArr(const std::string& key, Eigen::Vector3f vec)
+{
+ obj->add(key, ToArr(vec));
+}
+
+void SimpleJsonLoggerEntry::AddAsObj(const std::string& key, Eigen::Vector3f vec)
+{
+ obj->add(key, ToObj(vec));
+}
+
+void SimpleJsonLoggerEntry::AddAsArr(const std::string& key, Eigen::Matrix4f mat)
+{
+ obj->add(key, ToArr(mat));
+}
+
+void SimpleJsonLoggerEntry::Add(const std::string& key, const std::string& value)
+{
+ obj->add(key, JsonValue::Create(value));
+}
+
+void SimpleJsonLoggerEntry::Add(const std::string& key, float value)
+{
+ obj->add(key, JsonValue::Create(value));
+}
+
+
+JsonArrayPtr SimpleJsonLoggerEntry::ToArr(Eigen::Vector3f vec)
+{
+ JsonArrayPtr arr(new JsonArray);
+ arr->add(JsonValue::Create(vec(0)));
+ arr->add(JsonValue::Create(vec(1)));
+ arr->add(JsonValue::Create(vec(2)));
+ return arr;
+}
+
+JsonObjectPtr SimpleJsonLoggerEntry::ToObj(Eigen::Vector3f vec)
+{
+ JsonObjectPtr obj(new JsonObject);
+ obj->add("x", JsonValue::Create(vec(0)));
+ obj->add("y", JsonValue::Create(vec(1)));
+ obj->add("z", JsonValue::Create(vec(2)));
+ return obj;
+}
+
+JsonArrayPtr SimpleJsonLoggerEntry::ToArr(Eigen::Matrix4f mat)
+{
+ JsonArrayPtr arr(new JsonArray);
+ JsonArrayPtr row1(new JsonArray);
+ JsonArrayPtr row2(new JsonArray);
+ JsonArrayPtr row3(new JsonArray);
+ JsonArrayPtr row4(new JsonArray);
+
+ row1->add(JsonValue::Create(mat(0, 0)));
+ row1->add(JsonValue::Create(mat(0, 1)));
+ row1->add(JsonValue::Create(mat(0, 2)));
+ row1->add(JsonValue::Create(mat(0, 3)));
+
+ row2->add(JsonValue::Create(mat(1, 0)));
+ row2->add(JsonValue::Create(mat(1, 1)));
+ row2->add(JsonValue::Create(mat(1, 2)));
+ row2->add(JsonValue::Create(mat(1, 3)));
+
+ row3->add(JsonValue::Create(mat(2, 0)));
+ row3->add(JsonValue::Create(mat(2, 1)));
+ row3->add(JsonValue::Create(mat(2, 2)));
+ row3->add(JsonValue::Create(mat(2, 3)));
+
+ row4->add(JsonValue::Create(mat(3, 0)));
+ row4->add(JsonValue::Create(mat(3, 1)));
+ row4->add(JsonValue::Create(mat(3, 2)));
+ row4->add(JsonValue::Create(mat(3, 3)));
+
+ arr->add(row1);
+ arr->add(row2);
+ arr->add(row3);
+ arr->add(row4);
+
+ return arr;
+}
+
+void SimpleJsonLoggerEntry::AddTimestamp()
+{
+ IceUtil::Time now = IceUtil::Time::now();
+ obj->add("timestamp", JsonValue::Create(now.toDateTime()));
+}
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h
new file mode 100644
index 0000000000000000000000000000000000000000..0072eafe151eb74b5219ea7d6dd6b49f277dc643
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h
@@ -0,0 +1,58 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef armarx_SimpleJsonLogger_SimpleJsonLoggerEntry
+#define armarx_SimpleJsonLogger_SimpleJsonLoggerEntry
+
+#include <boost/shared_ptr.hpp>
+
+#include <ArmarXGui/libraries/StructuralJson/JsonObject.h>
+#include <Eigen/Dense>
+
+namespace armarx
+{
+ class SimpleJsonLoggerEntry;
+ typedef boost::shared_ptr<SimpleJsonLoggerEntry> SimpleJsonLoggerEntryPtr;
+
+ class SimpleJsonLoggerEntry
+ {
+ public:
+ SimpleJsonLoggerEntry();
+ void AddAsArr(const std::string& key, Eigen::Vector3f vec);
+ void AddAsObj(const std::string& key, Eigen::Vector3f vec);
+ void AddAsArr(const std::string& key, Eigen::Matrix4f mat);
+
+ void Add(const std::string& key, const std::string& value);
+ void Add(const std::string& key, float value);
+
+ static JsonArrayPtr ToArr(Eigen::Vector3f vec);
+ static JsonObjectPtr ToObj(Eigen::Vector3f vec);
+ static JsonArrayPtr ToArr(Eigen::Matrix4f mat);
+
+ void AddTimestamp();
+
+ JsonObjectPtr obj;
+ };
+}
+
+#endif // armarx_SimpleJsonLogger_SimpleJsonLoggerEntry
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/test/CMakeLists.txt b/source/RobotAPI/libraries/SimpleJsonLogger/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f37d4b381a496aa6f798dd0ed7d7bfc9b0369e28
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+#SET(LIBS ${LIBS} ArmarXCore SimpleJsonLogger)
+
+#armarx_add_test(SimpleJsonLoggerTest SimpleJsonLoggerTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/test/SimpleJsonLoggerTest.cpp b/source/RobotAPI/libraries/SimpleJsonLogger/test/SimpleJsonLoggerTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..15c777b73509c2ab2ccc7cd2ecc415819bb8152d
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/test/SimpleJsonLoggerTest.cpp
@@ -0,0 +1,36 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ReactiveGrasping::ArmarXObjects::SimpleJsonLogger
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2016
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE ReactiveGrasping::ArmarXLibraries::SimpleJsonLogger
+
+#define ARMARX_BOOST_TEST
+
+#include <ReactiveGrasping/Test.h>
+#include "../SimpleJsonLogger.h"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+
+ BOOST_CHECK_EQUAL(true, true);
+}