diff --git a/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx b/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
index f72fe0a6c6f8d86c56e7e4db9b711a5d2f0a25ec..c64ec99404f8ffc2249fcecd842e3d8771dd83f1 100644
--- a/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
+++ b/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
@@ -1,5 +1,5 @@
<?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>
diff --git a/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg b/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
index 9f81e34fd575229db796659be14e23cb54fc0f18..fbee75ed798eeccfb11b9d566e77a0d8f9efe8ab 100644
--- a/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
+++ b/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
diff --git a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
index a7d74099ab7a7bd0dbd84dad856412889ad09361..26b3b8d7a6a581a733ea27150923fec967383774 100644
--- a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
+++ b/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);
diff --git a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h
index cacb30f90c6f53a42c8825627eadc7b4cdcaba14..9cac7f358f940a97bd914551a1ed957b67bec60b 100644
--- a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h
+++ b/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()
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
index 5da836167939cf311ce9c5cab7da757982405f8a..3acae627c7627d3887b51e6b8e5412cdb50067f5 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
+++ b/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;
}
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
index 1561d2412dd90f964993f0f600a4459651f042da..90af7f0628f4cb864b629cea91b7631c15cd8cc9 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
+++ b/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
diff --git a/source/RobotAPI/interface/units/OrientedTactileSensorUnit.ice b/source/RobotAPI/interface/units/OrientedTactileSensorUnit.ice
index d36263d9544352992ceed4c949a52c2a465311f5..148e12f3909ca6c6617d31f679fc9c80eb51d18d 100644
--- a/source/RobotAPI/interface/units/OrientedTactileSensorUnit.ice
+++ b/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);
};