added linear acceleration

scenarios/OrientedTactileSensor/OrientedTactileSensor.scx

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

scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg

@@ -106,17 +106,25 @@ ArmarX.OrientedTactileSensorUnit.CalibrationData = 65524 3 12 65534 65534 1 1208
 # ArmarX.OrientedTactileSensorUnit.ObjectName = ""
 
 
+# ArmarX.OrientedTactileSensorUnit.SamplesAcceleration:  number of pressure values to differentiate
+#  Attributes:
+#  - Default:            20
+#  - Case sensitivity:   no
+#  - Required:           no
+ArmarX.OrientedTactileSensorUnit.SamplesAcceleration = 20
+
+
 # ArmarX.OrientedTactileSensorUnit.SamplesPressure:  number of pressure values to differentiate
 #  Attributes:
 #  - Default:            10
 #  - Case sensitivity:   no
 #  - Required:           no
-ArmarX.OrientedTactileSensorUnit.SamplesPressure = 10
+ArmarX.OrientedTactileSensorUnit.SamplesPressure = 20
 
 
 # ArmarX.OrientedTactileSensorUnit.SamplesRotation:  number of orientation values to differentiate
 #  Attributes:
-#  - Default:            10
+#  - Default:            20
 #  - Case sensitivity:   no
 #  - Required:           no
 ArmarX.OrientedTactileSensorUnit.SamplesRotation = 20

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 pressureRate, float rotationRate, float accelerationRate, 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, float accelerationRate, float accelx, float accely, float accelz, const TimestampBasePtr& timestamp, const Ice::Current&)
 {
     ScopedLock lock(dataMutex);
     TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
@@ -78,6 +78,7 @@ void OrientedTactileSensorUnitObserver::reportSensorValues(int id, float pressur
     offerOrUpdateDataField(channelName, "rotationRate", Variant(rotationRate), "current rotationRate");
     offerOrUpdateDataField(channelName, "pressureRate", Variant(pressureRate), "current pressureRate");
     offerOrUpdateDataField(channelName, "accelerationRate", Variant(accelerationRate), "current accelerationRate");
+    offerOrUpdateDataField(channelName, "acceleration", Variant(new Vector3(accelx, accely, accelz)), "current linear acceleration");
 }
 /* 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 pressureRate, float rotationRate, float accelerationRate, const TimestampBasePtr& timestamp, const Ice::Current&);
+        void reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float pressureRate, float rotationRate, float accelerationRate, float accelx, float accely, float accelz, const TimestampBasePtr& timestamp, const Ice::Current&);
 
         /**
          * @see PropertyUser::createPropertyDefinitions()

source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp

@@ -133,6 +133,7 @@ void OrientedTactileSensorUnit::run()
 
         //compute rotationRate
         float rotationRate = 0;
+
         //condition for inverse quaternion
         if ((pow(data.qw, 2) + pow(data.qx, 2) + pow(data.qy, 2) + pow(data.qz, 2)) != 0)
         {
@@ -175,7 +176,7 @@ void OrientedTactileSensorUnit::run()
             pressureRate = (samplePressure.pressure - oldsamplePressure.pressure) / (samplePressure.timestamp - oldsamplePressure.timestamp).toSecondsDouble();
         }
 
-        //compute accceleration Rate
+        //compute angular accceleration Rate
         float accelerationRate = 0;
         AccelerationRate sampleAcceleration;
         sampleAcceleration.timestamp = now;
@@ -193,13 +194,15 @@ void OrientedTactileSensorUnit::run()
             oldsampleAcceleration.rotationRate = samplesAcceleration.at(sampleIndexAcceleration).rotationRate;
             accelerationRate = (sampleAcceleration.rotationRate - oldsampleAcceleration.rotationRate) / (sampleAcceleration.timestamp - oldsampleAcceleration.timestamp).toSecondsDouble();
         }
-
+        if ((abs(pressureRate) > 50))
+        {
+            ARMARX_IMPORTANT << "contact";
+        }
 
         if (topicPrx)
         {
-            topicPrx->reportSensorValues(data.id, data.pressure, data.qw, data.qx, data.qy, data.qz, pressureRate, rotationRate, accelerationRate, nowTimestamp);
+            topicPrx->reportSensorValues(data.id, data.pressure, data.qw, data.qx, data.qy, data.qz, pressureRate, rotationRate, accelerationRate, data.accelx, data.accely, data.accelz, nowTimestamp);
         }
-
     }
 }
 
@@ -215,6 +218,9 @@ OrientedTactileSensorUnit::SensorData OrientedTactileSensorUnit::getValues(std::
     data.qx = std::stof(splitValues.at(3));
     data.qy = std::stof(splitValues.at(4));
     data.qz = std::stof(splitValues.at(5));
+    data.accelx = std::stof(splitValues.at(6));
+    data.accely = std::stof(splitValues.at(7));
+    data.accelz = std::stof(splitValues.at(8));
     return data;
 }
 
@@ -226,7 +232,6 @@ bool OrientedTactileSensorUnit::loadCalibration()
     while (arduinoLine.find("mode") == std::string::npos)
     {
         getline(arduino, arduinoLine, '\n');
-        //ARMARX_INFO << arduinoLine;
     }
     arduino.close();
 
@@ -239,7 +244,6 @@ bool OrientedTactileSensorUnit::loadCalibration()
     while (arduinoLine.find("calibration data") == std::string::npos)
     {
         getline(arduino, arduinoLine, '\n');
-        //ARMARX_INFO << arduinoLine;
     }
     arduino.close();
 
@@ -253,7 +257,6 @@ bool OrientedTactileSensorUnit::loadCalibration()
     while (arduinoLine.find("Calibration Sucessfull") == std::string::npos)
     {
         getline(arduino, arduinoLine, '\n');
-        //ARMARX_INFO << arduinoLine;
     }
     return true;
 }

source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h

@@ -81,6 +81,7 @@ namespace armarx
             int id;
             float pressure;
             float qw, qx, qy, qz;
+            float accelx, accely, accelz;
         };
 
         struct CalibrationData
@@ -105,6 +106,11 @@ namespace armarx
             IceUtil::Time timestamp;
             float rotationRate;
         };
+        struct LinAccRate
+        {
+            IceUtil::Time timestamp;
+            float accx, accy, accz;
+        };
 
     protected:
         virtual void onInitComponent();

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 pressureRate, float rotationRate, float accelerationRate, TimestampBase timestamp);
+        void reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float pressureRate, float rotationRate, float accelerationRate, float accelx, float accely, float accelz, TimestampBase timestamp);
     };