added pressureRate and proprieties for calibration

scenarios/OrientedTactileSensor/OrientedTactileSensor.scx

 <?xml version="1.0" encoding="utf-8"?>
-<scenario name="OrientedTactileSensor" lastChange="2017-03-01.15:11:23" creation="2017-02-27.01:48:55 PM" globalConfigName="./config/global.cfg" package="RobotAPI">
+<scenario name="OrientedTactileSensor" lastChange="2017-03-02.12:01:34 PM" 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>

scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg

@@ -74,6 +74,14 @@
 # ArmarX.EnableProfiling = 0
 
 
+# ArmarX.OrientedTactileSensorUnit.CalibrationData:  Sensor Register Data to calibrate the sensor
+#  Attributes:
+#  - 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.EnableProfiling:  enable profiler which is used for logging performance events
 #  Attributes:
 #  - Default:            0
@@ -114,6 +122,14 @@ ArmarX.OrientedTactileSensorUnit.SerialInterfaceDevice = /dev/ttyACM0
 # ArmarX.OrientedTactileSensorUnit.TopicName = OrientedTactileSensorValues
 
 
+# ArmarX.OrientedTactileSensorUnit.calibrateSensor:  
+#  Attributes:
+#  - Default:            1
+#  - Case sensitivity:   no
+#  - Required:           no
+ArmarX.OrientedTactileSensorUnit.calibrateSensor = 0
+
+
 # ArmarX.RedirectStdout:  Redirect std::cout and std::cerr to ArmarXLog
 #  Attributes:
 #  - Default:            1

source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp

@@ -58,7 +58,7 @@ void OrientedTactileSensorUnitObserver::onExitObserver()
     //debugDrawerPrx->removeLineVisu("IMU", "acceleration");
 }
 
-void OrientedTactileSensorUnitObserver::reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float rotationRate, const TimestampBasePtr& timestamp, const Ice::Current&)
+void OrientedTactileSensorUnitObserver::reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float pressureRate, float rotationRate, const TimestampBasePtr& timestamp, const Ice::Current&)
 {
     ScopedLock lock(dataMutex);
     TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
@@ -76,6 +76,7 @@ void OrientedTactileSensorUnitObserver::reportSensorValues(int id, float pressur
     QuaternionPtr orientationQuaternion =  new Quaternion(qw, qx, qy, qz);
     offerOrUpdateDataField(channelName, "orientation", orientationQuaternion, "current oriantation");
     offerOrUpdateDataField(channelName, "rotationRate", Variant(rotationRate), "current rotationRate");
+    offerOrUpdateDataField(channelName, "pressureRate", Variant(pressureRate), "current pressureRate");
 }
 /* TODO: for integration with debug drawer
     updateChannel(device);

source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h

@@ -72,7 +72,7 @@ namespace armarx
         virtual void onConnectObserver();
         virtual void onExitObserver();
 
-        void reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float rotationRate, const TimestampBasePtr& timestamp, const Ice::Current&);
+        void reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float pressureRate, float rotationRate, const TimestampBasePtr& timestamp, const Ice::Current&);
 
         /**
          * @see PropertyUser::createPropertyDefinitions()

source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp

@@ -3,14 +3,19 @@
 #include <sys/ioctl.h>
 #include <fcntl.h>
 #include <math.h>
+#include <ArmarXCore/core/application/Application.h>
 #include <ArmarXCore/observers/variant/TimestampVariant.h>
 
 using namespace armarx;
 
 OrientedTactileSensorUnit::OrientedTactileSensorUnit()
 {
-    maxSamples = 10;
-    sampleIndex = 0;
+    maxSamplesRotation = 10;
+    sampleIndexRotation = 0;
+    maxSamplesPressure = 10;
+    sampleIndexPressure = 0;
+    maxSamplesRotationRate = 10;
+    sampleIndexRotationRate = 0;
 }
 
 void OrientedTactileSensorUnit::onInitComponent()
@@ -61,13 +66,13 @@ void OrientedTactileSensorUnit::onInitComponent()
     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 (CALIBRATE_ON)
+    if (getProperty<bool>("calibrateSensor").getValue())
     {
         //calibrate
+
         while (arduinoLine.find("mode") == std::string::npos)
         {
             getline(arduino, arduinoLine, '\n');
-            ARMARX_INFO << arduinoLine;
         }
         arduino.close();
 
@@ -82,12 +87,16 @@ void OrientedTactileSensorUnit::onInitComponent()
             getline(arduino, arduinoLine, '\n');
             ARMARX_INFO << arduinoLine;
         }
-        ARMARX_IMPORTANT << "calibrated sensor";
+        getline(arduino, arduinoLine, '\n');
+        if (getCalibrationValues(arduinoLine))
+        {
+            ARMARX_IMPORTANT << "calibrated sensor";
+        }
     }
     else
     {
         //load calibration
-        ARMARX_INFO << "load";
+        ARMARX_INFO << "load calibration data " << getProperty<std::string>("CalibrationData").getValue();
         if (loadCalibration())
         {
             ARMARX_IMPORTANT << "loaded calibration";
@@ -119,26 +128,50 @@ void OrientedTactileSensorUnit::run()
         ARMARX_INFO << line;
         SensorData data = getValues(line.c_str());
         IceUtil::Time now = IceUtil::Time::now();
-        TimestampVariantPtr nowTimestamp = new TimestapressurempVariant(now);
-
+        TimestampVariantPtr nowTimestamp = new TimestampVariant(now);
         float rotationRate = 0;
-        std::pair<IceUtil::Time, Eigen::Quaternionf> sample = std::make_pair(now, Eigen::Quaternionf(data.qw, data.qx, data.qy, data.qz));
-        if (samples.size() < maxSamples)
+        //condition for inverse quaternion
+        if ((pow(data.qw, 2) + pow(data.qx, 2) + pow(data.qy, 2) + pow(data.qz, 2)) != 0)
+        {
+            RotationRate sampleRotation;
+            sampleRotation.timestamp = now;
+            sampleRotation.orientation =  Eigen::Quaternionf(data.qw, data.qx, data.qy, data.qz);
+            if (samplesRotation.size() < maxSamplesRotation)
+            {
+                samplesRotation.push_back(sampleRotation);
+            }
+            else
+            {
+                samplesRotation[sampleIndexRotation].timestamp = sampleRotation.timestamp;
+                samplesRotation[sampleIndexRotation].orientation = sampleRotation.orientation;
+                sampleIndexRotation = (sampleIndexRotation + 1) % maxSamplesRotation;
+                RotationRate oldsampleRotation;
+                oldsampleRotation.timestamp = samplesRotation.at(sampleIndexRotation).timestamp;
+                oldsampleRotation.orientation = samplesRotation.at(sampleIndexRotation).orientation;
+                Eigen::AngleAxisf aa(sampleRotation.orientation * oldsampleRotation.orientation.inverse());
+                rotationRate = aa.angle() / (sampleRotation.timestamp - oldsampleRotation.timestamp).toSecondsDouble();
+            }
+        }
+        float pressureRate = 0;
+        PressureRate samplePressure;
+        samplePressure.timestamp = now;
+        samplePressure.pressure = data.pressure;
+        if (samplesPressure.size() < maxSamplesPressure)
         {
-            samples.push_back(sample);
+            samplesPressure.push_back(samplePressure);
         }
         else
         {
-            samples[sampleIndex] = sample;
-            sampleIndex = (sampleIndex + 1) % maxSamples;
-            std::pair<IceUtil::Time, Eigen::Quaternionf> oldsample = samples.at(sampleIndex);
-            Eigen::AngleAxisf aa(oldsample.second.inverse() * sample.second);
-            rotationRate = aa.angle() / (sample.first - oldsample.first).toSecondsDouble();
+            samplesPressure[sampleIndexPressure] = samplePressure;
+            sampleIndexPressure = (sampleIndexPressure + 1) % maxSamplesPressure;
+            PressureRate oldsamplePressure;
+            oldsamplePressure.timestamp = samplesPressure.at(sampleIndexPressure).timestamp;
+            oldsamplePressure.pressure = samplesPressure.at(sampleIndexPressure).pressure;
+            pressureRate = (samplePressure.pressure - oldsamplePressure.pressure) / (samplePressure.timestamp - oldsamplePressure.timestamp).toSecondsDouble();
         }
-
         if (topicPrx)
         {
-            topicPrx->reportSensorValues(data.id, data.pressure, data.qw, data.qx, data.qy, data.qz, rotationRate, nowTimestamp);
+            topicPrx->reportSensorValues(data.id, data.pressure, data.qw, data.qx, data.qy, data.qz, pressureRate, rotationRate, nowTimestamp);
         }
 
     }
@@ -159,31 +192,15 @@ OrientedTactileSensorUnit::SensorData OrientedTactileSensorUnit::getValues(std::
     return data;
 }
 
-/*void OrientedTactileSensorUnit::getCalibrationValues(std::string line)
-{
-    std::vector<std::string> splitValues;
-    boost::split(splitValues, line, boost::is_any_of(" "));
-    calibration.accel_offset_x = stoi(splitValues.at(0));
-    calibration.accel_offset_y = stoi(splitValues.at(1));
-    calibration.accel_offset_z = stoi(splitValues.at(2));
-    calibration.gyro_offset_x = stoi(splitValues.at(3));
-    calibration.gyro_offset_y = stoi(splitValues.at(4));
-    calibration.gyro_offset_z = stoi(splitValues.at(5));
-    calibration.mag_offset_x = stoi(splitValues.at(6));
-    calibration.mag_offset_y = stoi(splitValues.at(7));
-    calibration.mag_offset_z = stoi(splitValues.at(8));
-    calibration.accel_radius = stoi(splitValues.at(9));
-    calibration.mag_radius = stoi(splitValues.at(10));
-}
-*/
 std::string space = " ";
 bool OrientedTactileSensorUnit::loadCalibration()
 {
-    std::string calibrationStream = getCalibrationValues();
+    std::string calibrationStream = getProperty<std::string>("CalibrationData").getValue();
     std::string arduinoLine;
     while (arduinoLine.find("mode") == std::string::npos)
     {
         getline(arduino, arduinoLine, '\n');
+        //ARMARX_INFO << arduinoLine;
     }
     arduino.close();
 
@@ -196,6 +213,7 @@ bool OrientedTactileSensorUnit::loadCalibration()
     while (arduinoLine.find("calibration data") == std::string::npos)
     {
         getline(arduino, arduinoLine, '\n');
+        //ARMARX_INFO << arduinoLine;
     }
     arduino.close();
 
@@ -209,28 +227,32 @@ bool OrientedTactileSensorUnit::loadCalibration()
     while (arduinoLine.find("Calibration Sucessfull") == std::string::npos)
     {
         getline(arduino, arduinoLine, '\n');
+        //ARMARX_INFO << arduinoLine;
     }
     return true;
 }
 
-std::string OrientedTactileSensorUnit::getCalibrationValues()
+bool OrientedTactileSensorUnit::getCalibrationValues(std::string line)
 {
-    calibration.accel_offset_x = 65524;
-    calibration.accel_offset_y = 3;
-    calibration.accel_offset_z = 12;
-    calibration.gyro_offset_x = 65534;
-    calibration.gyro_offset_y = 65534;
-    calibration.gyro_offset_z = 1;
-    calibration.mag_offset_x = 1208;
-    calibration.mag_offset_y = 119;
-    calibration.mag_offset_z = 58726;
-    calibration.accel_radius = 1000;
-    calibration.mag_radius = 943;
+    std::vector<std::string> splitValues;
+    boost::split(splitValues, line, boost::is_any_of(" "));
+    calibration.accel_offset_x = stoi(splitValues.at(0));
+    calibration.accel_offset_y = stoi(splitValues.at(1));
+    calibration.accel_offset_z = stoi(splitValues.at(2));
+    calibration.gyro_offset_x = stoi(splitValues.at(3));
+    calibration.gyro_offset_y = stoi(splitValues.at(4));
+    calibration.gyro_offset_z = stoi(splitValues.at(5));
+    calibration.mag_offset_x = stoi(splitValues.at(6));
+    calibration.mag_offset_y = stoi(splitValues.at(7));
+    calibration.mag_offset_z = stoi(splitValues.at(8));
+    calibration.accel_radius = stoi(splitValues.at(9));
+    calibration.mag_radius = stoi(splitValues.at(10));
     std::string space = " ";
     std::string calibrationStream = "";
     calibrationStream = calibrationStream + std::to_string(calibration.accel_offset_x) + space + std::to_string(calibration.accel_offset_y) + space + std::to_string(calibration.accel_offset_z) + space + std::to_string(calibration.gyro_offset_x) + space + std::to_string(calibration.gyro_offset_y) + space +
                         std::to_string(calibration.gyro_offset_z) + space + std::to_string(calibration.mag_offset_x) + space + std::to_string(calibration.mag_offset_y) + space + std::to_string(calibration.mag_offset_z) + space + std::to_string(calibration.accel_radius) + space + std::to_string(calibration.mag_radius) + space;
-    return calibrationStream;
+    ARMARX_IMPORTANT << "calibration data: " << calibrationStream ;
+    return true;
 }
 
 

source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h

@@ -34,6 +34,15 @@ namespace armarx
                 "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<bool>(
+                "calibrateSensor",
+                "false"
+                "Set true to calibrate the sensor and get calibration data and false to use existent calibration data");
         }
     };
 
@@ -66,6 +75,24 @@ namespace armarx
             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;
+        };
+
     protected:
         virtual void onInitComponent();
         virtual void onConnectComponent();
@@ -79,15 +106,21 @@ namespace armarx
         OrientedTactileSensorUnitInterfacePrx interfacePrx;
 
         void run();
-        SensorData getValues(std::string s);
-        std::string getCalibrationValues();
+        SensorData getValues(std::string line);
+        bool getCalibrationValues(std::string line);
         bool loadCalibration();
         int fd;
         CalibrationData calibration;
 
-        std::vector<std::pair<IceUtil::Time, Eigen::Quaternionf>> samples;
-        int maxSamples;
-        int sampleIndex;
+        std::vector<RotationRate> samplesRotation;
+        std::vector<PressureRate> samplesPressure;
+        std::vector<AccelerationRate> samplesRotationRate;
+        int maxSamplesRotation;
+        int sampleIndexRotation;
+        int maxSamplesPressure;
+        int sampleIndexPressure;
+        int maxSamplesRotationRate;
+        int sampleIndexRotationRate;
     };
 }
 #endif // SENSORPACKAGEUNIT_H

source/RobotAPI/interface/units/OrientedTactileSensorUnit.ice

@@ -47,7 +47,7 @@ module armarx
 
    interface OrientedTactileSensorUnitListener
     {
-        void reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float rotationRate, TimestampBase timestamp);
+        void reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float pressureRate, float rotationRate, TimestampBase timestamp);
     };