created SensorPackage

source/RobotAPI/applications/CMakeLists.txt

@@ -22,3 +22,5 @@ add_subdirectory(XsensIMU)
 add_subdirectory(InertialMeasurementUnitObserver)
 
 add_subdirectory(ViewSelection)
+
+add_subdirectory(SensorPackageUnit)

source/RobotAPI/applications/SensorPackageUnit/CMakeLists.txt

+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}")

source/RobotAPI/applications/SensorPackageUnit/main.cpp

+#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");
+}

source/RobotAPI/drivers/CMakeLists.txt

 add_subdirectory(WeissHapticSensor)
 add_subdirectory(XsensIMU)
+add_subdirectory(SensorPackage)

source/RobotAPI/drivers/SensorPackage/CMakeLists.txt

+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}")

source/RobotAPI/drivers/SensorPackage/SensorPackageUnit.cpp

+#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;
+}
+*/
+
+
+
+
+
+

source/RobotAPI/drivers/SensorPackage/SensorPackageUnit.h

+#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

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
 

source/RobotAPI/interface/units/SensorPackageUnit.ice

+/*
+ * 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