diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index 736267ef643f75f212ba27af7efa84ff362f9464..136b7d17ab16700ac9193d4ca78b0b81c362dbfa 100644
--- a/source/RobotAPI/applications/CMakeLists.txt
+++ b/source/RobotAPI/applications/CMakeLists.txt
@@ -22,3 +22,5 @@ add_subdirectory(XsensIMU)
add_subdirectory(InertialMeasurementUnitObserver)
add_subdirectory(ViewSelection)
+
+add_subdirectory(SensorPackageUnit)
diff --git a/source/RobotAPI/applications/SensorPackageUnit/CMakeLists.txt b/source/RobotAPI/applications/SensorPackageUnit/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..abfb42d97847e44a3cf15e39a8937d0346f3ee20
--- /dev/null
+++ b/source/RobotAPI/applications/SensorPackageUnit/CMakeLists.txt
@@ -0,0 +1,15 @@
+armarx_component_set_name("SensorPackageUnitApp")
+
+find_package(Eigen3 QUIET)
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+
+if (Eigen3_FOUND)
+ include_directories(
+ ${Eigen3_INCLUDE_DIR})
+endif()
+
+set(COMPONENT_LIBS RobotAPIUnits SensorPackage )
+
+set(EXE_SOURCE main.cpp)
+
+armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/SensorPackageUnit/main.cpp b/source/RobotAPI/applications/SensorPackageUnit/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c8d30362e718bdb30ea2cbaca5aa3e3742ebc2cb
--- /dev/null
+++ b/source/RobotAPI/applications/SensorPackageUnit/main.cpp
@@ -0,0 +1,8 @@
+#include <ArmarXCore/core/application/Application.h>
+#include <RobotAPI/drivers/SensorPackage/SensorPackageUnit.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+ return armarx::runSimpleComponentApp < armarx::SensorPackageUnit > (argc, argv, "SensorPackageUnit");
+}
diff --git a/source/RobotAPI/drivers/CMakeLists.txt b/source/RobotAPI/drivers/CMakeLists.txt
index ce2834d8731aa2a82f549313634a7d28b2277084..c86f60a30c639a7e58d374286873bc422295d739 100644
--- a/source/RobotAPI/drivers/CMakeLists.txt
+++ b/source/RobotAPI/drivers/CMakeLists.txt
@@ -1,2 +1,3 @@
add_subdirectory(WeissHapticSensor)
add_subdirectory(XsensIMU)
+add_subdirectory(SensorPackage)
diff --git a/source/RobotAPI/drivers/SensorPackage/CMakeLists.txt b/source/RobotAPI/drivers/SensorPackage/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..889fbb526ba88a183e3c6aea261d0d74b5a23c81
--- /dev/null
+++ b/source/RobotAPI/drivers/SensorPackage/CMakeLists.txt
@@ -0,0 +1,25 @@
+armarx_set_target("SensorPackage Library: SensorPackage")
+
+find_package(Eigen3 QUIET)
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+
+if (Eigen3_FOUND)
+ include_directories(
+ ${Eigen3_INCLUDE_DIR})
+endif()
+
+set(LIB_NAME SensorPackage)
+
+
+
+set(LIBS RobotAPIUnits ArmarXCoreObservers ArmarXCoreEigen3Variants)
+
+set(LIB_FILES
+ SensorPackageUnit.cpp
+)
+set(LIB_HEADERS
+ SensorPackageUnit.h
+)
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/drivers/SensorPackage/SensorPackageUnit.cpp b/source/RobotAPI/drivers/SensorPackage/SensorPackageUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c7f29439ec33bb6e6096fce9c6bcf4c68066cce5
--- /dev/null
+++ b/source/RobotAPI/drivers/SensorPackage/SensorPackageUnit.cpp
@@ -0,0 +1,185 @@
+#include "SensorPackageUnit.h"
+#include <termios.h>
+#include <sys/ioctl.h>
+#include <fcntl.h>
+#include <math.h>
+
+using namespace armarx;
+
+SensorPackageUnit::SensorPackageUnit()
+ : recording(false)
+{
+}
+
+void SensorPackageUnit::startRecording(const std::string& recordingName, const Ice::Current& c)
+{
+ ARMARX_INFO << "startRecording";
+ std::string sensorDataRecordingFileName = recordingName + "_data.txt";
+ outputFile.open(sensorDataRecordingFileName);
+ recording = true;
+}
+
+void SensorPackageUnit::stopRecording(const Ice::Current& c)
+{
+ recording = false;
+ ARMARX_INFO << "stopRecording";
+ ScopedLock lock(fileMutex);
+ outputFile.close();
+}
+
+bool SensorPackageUnit::isComponentOnline(const Ice::Current& c)
+{
+ return true;
+}
+
+void SensorPackageUnit::onInitComponent()
+{
+ offeringTopic("SensorPackageUnit");
+ recording = false;
+
+ //open serial port
+ std::string portname = getProperty<std::string>("SerialInterfaceDevice").getValue();
+ arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::in);
+
+ fd = open(portname.c_str(), O_RDONLY | O_NOCTTY);
+ struct termios toptions;
+ /* Get currently set options for the tty */
+ tcgetattr(fd, &toptions);
+ /* Set custom options */
+ cfsetispeed(&toptions, B115200);
+ cfsetospeed(&toptions, B115200);
+ /* 8 bits, no parity, no stop bits */
+ toptions.c_lflag = 0;
+ toptions.c_iflag = 0;
+ toptions.c_oflag = 0;
+ /* commit the options */
+ tcsetattr(fd, TCSANOW, &toptions);
+ /* Wait for the Arduino to reset */
+ usleep(1000 * 1000);
+ /* Flush anything already in the serial buffer */
+ tcflush(fd, TCIFLUSH);
+
+ ARMARX_INFO << "opening device " << getProperty<std::string>("SerialInterfaceDevice").getValue();
+
+ if (!arduino.is_open())
+ {
+
+ throw LocalException("Cannot open Arduino on ") << getProperty<std::string>("SerialInterfaceDevice").getValue();
+ }
+
+ ARMARX_INFO << "Arduino restarts, please wait ...";
+
+ //wait for the Arduino to reboot
+ usleep(4000000);
+
+ //wait for the IMU to be calibrated
+ std::string arduinoLine;
+ while (arduinoLine.find("Calibratrion Sucessfull") == std::string::npos)
+ {
+ getline(arduino, arduinoLine, '\n');
+ ARMARX_INFO << "waiting for IMU calibration - this can take some time";
+ }
+
+ ARMARX_INFO << "IMU calibration finished";
+
+ readTask = new RunningTask<SensorPackageUnit>(this, &SensorPackageUnit::run);
+ readTask->start();
+
+}
+
+void SensorPackageUnit::onConnectComponent()
+{
+ //std::string topicName = getProperty<std::string>("ArduinoIMUUnit").getValue();
+ std::string topicName = getProperty<std::string>("TopicName").getValue();
+ listenerSensorPackage = getTopic<SensorPackageUnitListenerPrx>(topicName);
+ ARMARX_INFO << "Topic Name given" << flush;
+}
+
+PropertyDefinitionsPtr SensorPackageUnit::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new SensorPackageUnitPropertyDefinitions(
+ getConfigIdentifier()));
+}
+
+void SensorPackageUnit::run()
+{
+ std::string line;
+ while (readTask->isRunning())
+ {
+ getline(arduino, line, '\n');
+ if (recording)
+ {
+ ScopedLock lock(fileMutex);
+
+
+ // get sensor data from serial
+ getValues(line.c_str());
+
+ outputFile << IceUtil::Time::now().toDateTime() << " " << "Id: " << sensorData[0] << " Pressure: " << sensorData[1] << " IMU: " << sensorData[2] << " " << sensorData[3] << " " << sensorData[4] << " " << sensorData[5] << "\n";
+ ARMARX_INFO << "Id: " << sensorData[0] << " Pressure: " << sensorData[1] << " IMU: " << sensorData[2] << " " << sensorData[3] << " " << sensorData[4] << " " << sensorData[5] << "\n";
+ //outputFile << IceUtil::Time::now().toDateTime() << " " << line << "\n";
+ //ARMARX_INFO << line;
+
+ outputFile.flush();
+
+ //listenerIMU->reportForceSensorValues(sensorData[0], sensorData[1], sensorData[2], sensorData[3], sensorData[4], sensorData[5]);
+ }
+ }
+}
+
+// get imu values from incoming string
+void SensorPackageUnit::getValues(std::string line)
+{
+ std::string erasedLine = line.erase(0, 6);
+ char* erasedL = new char[erasedLine.length() + 1];
+ strcpy(erasedL, erasedLine.c_str());
+
+ // split string at blank space
+ const char* cutter = " ";
+ char* cutPieces = strtok(erasedL, cutter);
+ while (cutPieces != NULL)
+ {
+ for (int i = 0; i < sizeArray; i++)
+ {
+ sensorData[i] = atof(cutPieces);
+ cutPieces = strtok(NULL, cutter);
+ }
+
+ }
+ delete [] erasedL;
+}
+
+/*void SensorPackageUnit::angleDiff()
+{
+ double diff;
+ for (int i = 0; i < 3 ; i++)
+ {
+ diff = std::fabs(std::fabs(imuValEuler[i]) - std::fabs(imuValEuler[i + 3]));
+ imuDiff[i] = std::min(diff, 360 - diff);
+ }
+
+ imuDiff[0] = 210 - imuDiff[0];
+}
+
+void ArduinoIMUUnit::quaternionToEuler(float w, float x, float y, float z)
+{
+ //calculation roll in radians
+ float ro = atan2((2 * (w * x + y * z)), (1 - (2 * ((x * x) + (y * y)))));
+ //transformation to degree
+ roll = (ro * 180) / PI;
+ //calculation pitch in radians
+ float pit = asin(2 * (w * y - z * y));
+ //transformation to degree
+ pitch = (pit * 180) / PI;
+ //calculation yaw in radians
+ float yw = atan2((2 * (w * z + x * y)), (1 - (2 * ((y * y) + (z * z)))));
+ //transformation to degree
+ yaw = (yw * 180) / PI;
+}
+*/
+
+
+
+
+
+
diff --git a/source/RobotAPI/drivers/SensorPackage/SensorPackageUnit.h b/source/RobotAPI/drivers/SensorPackage/SensorPackageUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..986adb44851eca76f72f1221564f8fe16964703a
--- /dev/null
+++ b/source/RobotAPI/drivers/SensorPackage/SensorPackageUnit.h
@@ -0,0 +1,86 @@
+#ifndef SENSORPACKAGEUNIT_H
+#define SENSORPACKAGEUNIT_H
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/units/UnitInterface.h>
+#include <RobotAPI/interface/units/SensorPackageUnit.h> //.ice for datastructure
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
+#include <netinet/in.h>
+#include <fstream>
+#include <stdio.h>
+#include <boost/date_time/posix_time/posix_time.hpp>
+//#include <MMMCapture/interface/units/MMMCaptureSensor.h>
+//#include <ArmarXCore/interface/observers/ObserverInterface.h>
+
+
+namespace armarx
+{
+ class SensorPackageUnitPropertyDefinitions:
+ public ComponentPropertyDefinitions
+ {
+ public:
+ SensorPackageUnitPropertyDefinitions(std::string prefix):
+ ComponentPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::string>(
+ "SerialInterfaceDevice",
+ "/dev/ttyACM0",
+ "The serial device the arduino is connected to.");
+
+ defineOptionalProperty<std::string>(
+ "TopicName",
+ "SensorPackageUnit",
+ "Name of the topic on which the sensor values are provided");
+
+ }
+ };
+
+ /**
+ * @class SensorPackageUnit
+ * @brief ArmarX wrapper for an arduino due with one BNO055 IMU and one BMP280 pressure sensor
+ *
+ */
+ class SensorPackageUnit:
+ virtual public armarx::Component
+ //virtual public MMMCaptureSensorInterface
+ {
+ public:
+ SensorPackageUnit();
+ virtual void startRecording(const std::string& recordingName, const Ice::Current& c);
+ virtual void stopRecording(const Ice::Current& c);
+ bool isComponentOnline(const Ice::Current& c);
+
+
+ virtual std::string getDefaultName() const
+ {
+ return "SensorPackageUnit";
+ }
+
+ protected:
+ virtual void onInitComponent();
+ virtual void onConnectComponent();
+
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ private:
+ std::fstream arduino;
+ std::ofstream outputFile;
+ Mutex fileMutex;
+ RunningTask<SensorPackageUnit>::pointer_type readTask;
+ SensorPackageUnitListenerPrx listenerSensorPackage;
+
+ void periodicExec();
+ bool recording;
+ void run();
+ void getValues(std::string s);
+
+ static const int sizeArray = 6;
+ float sensorData[sizeArray];
+ int fd;
+
+ };
+}
+
+
+#endif // SENSORPACKAGEUNIT_H
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 044aadda547e4b93c3b1672a85af91a208fec47f..e7eec6b50ef3cd857d53a15760cca477fc2625a0 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -36,7 +36,7 @@ set(SLICE_FILES
units/TCPMoverUnitInterface.ice
units/UnitInterface.ice
units/ATINetFTUnit.ice
-
+ units/SensorPackageUnit.ice
components/ViewSelectionInterface.ice
diff --git a/source/RobotAPI/interface/units/SensorPackageUnit.ice b/source/RobotAPI/interface/units/SensorPackageUnit.ice
new file mode 100644
index 0000000000000000000000000000000000000000..0ba11904d2327c9c6fa29e8e9744fded32e6fa0c
--- /dev/null
+++ b/source/RobotAPI/interface/units/SensorPackageUnit.ice
@@ -0,0 +1,65 @@
+/*
+ * 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 RobotAPI
+ * @author Martin Do ( martin dot do at kit dot edu )
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_UNITS_SENSORPACKAGE_SLICE_
+#define _ARMARX_ROBOTAPI_UNITS_SENSORPACKAGE_SLICE_
+
+#include <RobotAPI/interface/core/PoseBase.ice>
+#include <RobotAPI/interface/units/UnitInterface.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
+
+module armarx
+{
+ /**
+ * Will be thrown, if a Arduino error occurs.
+ */
+ exception ArduinoException extends UserException
+ {
+ int errorCode;
+ };
+
+
+ interface SensorPackageUnitInterface extends SensorActorUnitInterface
+ {
+ //void startRecording(string customName);
+
+ //void stopRecording();
+
+ bool isComponentOnline();
+ };
+
+ interface SensorPackageUnitListener
+ {
+ void reportSensorValues(float id, float pressure, float posw, float posx, float posy, float posz);
+ //void reportForceSensorValues(float pos1x, float pos1y, float pos1z, float quaternion1,
+ // float pos2x, float pos2y, float pos2z, float quaternion2);
+
+ };
+
+
+ interface SensorPackageUnitObserverInterface extends ObserverInterface, SensorPackageUnitListener
+ {
+
+ };
+};
+
+#endif