From d9426456f9d0c6f60c07ba2721b17d0e3eea31dd Mon Sep 17 00:00:00 2001
From: andreeatulbure <andreea_tulbure@yahoo.de>
Date: Thu, 2 Mar 2017 13:36:54 +0100
Subject: [PATCH] added angular acceleration and maxSamples as proprieties
---
.../OrientedTactileSensor.scx | 2 +-
.../config/OrientedTactileSensorUnitApp.cfg | 16 ++++++++
.../OrientedTactileSensorUnitObserver.cpp | 3 +-
.../units/OrientedTactileSensorUnitObserver.h | 2 +-
.../OrientedTactileSensorUnit.cpp | 38 ++++++++++++++++---
.../OrientedTactileSensorUnit.h | 25 +++++++++---
.../units/OrientedTactileSensorUnit.ice | 2 +-
7 files changed, 72 insertions(+), 16 deletions(-)
diff --git a/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx b/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
index c64ec9940..e1224df59 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-02.12:01:34 PM" creation="2017-02-27.01:48:55 PM" globalConfigName="./config/global.cfg" package="RobotAPI">
+<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">
<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 fbee75ed7..d7b52516c 100644
--- a/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
+++ b/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
@@ -106,6 +106,22 @@ ArmarX.OrientedTactileSensorUnit.CalibrationData = 65524 3 12 65534 65534 1 1208
# ArmarX.OrientedTactileSensorUnit.ObjectName = ""
+# ArmarX.OrientedTactileSensorUnit.SamplesPressure: number of pressure values to differentiate
+# Attributes:
+# - Default: 10
+# - Case sensitivity: no
+# - Required: no
+ArmarX.OrientedTactileSensorUnit.SamplesPressure = 10
+
+
+# ArmarX.OrientedTactileSensorUnit.SamplesRotation: number of orientation values to differentiate
+# Attributes:
+# - Default: 10
+# - Case sensitivity: no
+# - Required: no
+ArmarX.OrientedTactileSensorUnit.SamplesRotation = 20
+
+
# ArmarX.OrientedTactileSensorUnit.SerialInterfaceDevice: The serial device the arduino is connected to.
# Attributes:
# - Default: /dev/ttyACM0
diff --git a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
index 26b3b8d7a..852860c26 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 pressureRate, 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, float accelerationRate, const TimestampBasePtr& timestamp, const Ice::Current&)
{
ScopedLock lock(dataMutex);
TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
@@ -77,6 +77,7 @@ void OrientedTactileSensorUnitObserver::reportSensorValues(int id, float pressur
offerOrUpdateDataField(channelName, "orientation", orientationQuaternion, "current oriantation");
offerOrUpdateDataField(channelName, "rotationRate", Variant(rotationRate), "current rotationRate");
offerOrUpdateDataField(channelName, "pressureRate", Variant(pressureRate), "current pressureRate");
+ offerOrUpdateDataField(channelName, "accelerationRate", Variant(accelerationRate), "current accelerationRate");
}
/* 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 9cac7f358..716bdf828 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 pressureRate, 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, float accelerationRate, 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 3acae627c..70f80355e 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
@@ -10,16 +10,18 @@ using namespace armarx;
OrientedTactileSensorUnit::OrientedTactileSensorUnit()
{
- maxSamplesRotation = 10;
+
sampleIndexRotation = 0;
- maxSamplesPressure = 10;
sampleIndexPressure = 0;
- maxSamplesRotationRate = 10;
- sampleIndexRotationRate = 0;
+ sampleIndexAcceleration = 0;
}
void OrientedTactileSensorUnit::onInitComponent()
{
+ maxSamplesRotation = stoi(getProperty<std::string>("SamplesRotation").getValue());
+ maxSamplesPressure = stoi(getProperty<std::string>("SamplesPressure").getValue());
+ maxSamplesAcceleration = stoi(getProperty<std::string>("SamplesAcceleration").getValue());
+
std::string topicName = getProperty<std::string>("TopicName").getValue();
offeringTopic(topicName);
@@ -125,10 +127,11 @@ void OrientedTactileSensorUnit::run()
{
std::string line;
getline(arduino, line, '\n');
- ARMARX_INFO << line;
SensorData data = getValues(line.c_str());
IceUtil::Time now = IceUtil::Time::now();
TimestampVariantPtr nowTimestamp = new TimestampVariant(now);
+
+ //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)
@@ -152,6 +155,8 @@ void OrientedTactileSensorUnit::run()
rotationRate = aa.angle() / (sampleRotation.timestamp - oldsampleRotation.timestamp).toSecondsDouble();
}
}
+
+ //compute pressureRate
float pressureRate = 0;
PressureRate samplePressure;
samplePressure.timestamp = now;
@@ -169,9 +174,30 @@ void OrientedTactileSensorUnit::run()
oldsamplePressure.pressure = samplesPressure.at(sampleIndexPressure).pressure;
pressureRate = (samplePressure.pressure - oldsamplePressure.pressure) / (samplePressure.timestamp - oldsamplePressure.timestamp).toSecondsDouble();
}
+
+ //compute accceleration Rate
+ float accelerationRate = 0;
+ AccelerationRate sampleAcceleration;
+ sampleAcceleration.timestamp = now;
+ sampleAcceleration.rotationRate = rotationRate;
+ if (samplesAcceleration.size() < maxSamplesAcceleration)
+ {
+ samplesAcceleration.push_back(sampleAcceleration);
+ }
+ else
+ {
+ samplesAcceleration[sampleIndexAcceleration] = sampleAcceleration;
+ sampleIndexAcceleration = (sampleIndexAcceleration + 1) % maxSamplesAcceleration;
+ AccelerationRate oldsampleAcceleration;
+ oldsampleAcceleration.timestamp = samplesAcceleration.at(sampleIndexAcceleration).timestamp;
+ oldsampleAcceleration.rotationRate = samplesAcceleration.at(sampleIndexAcceleration).rotationRate;
+ accelerationRate = (sampleAcceleration.rotationRate - oldsampleAcceleration.rotationRate) / (sampleAcceleration.timestamp - oldsampleAcceleration.timestamp).toSecondsDouble();
+ }
+
+
if (topicPrx)
{
- topicPrx->reportSensorValues(data.id, data.pressure, data.qw, data.qx, data.qy, data.qz, pressureRate, rotationRate, nowTimestamp);
+ topicPrx->reportSensorValues(data.id, data.pressure, data.qw, data.qx, data.qy, data.qz, pressureRate, rotationRate, accelerationRate, nowTimestamp);
}
}
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
index 90af7f062..0e6c8ced6 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
@@ -10,11 +10,8 @@
#include <fstream>
#include <stdio.h>
#include <boost/date_time/posix_time/posix_time.hpp>
-//#include "RobotAPI/components/units/SensorActorUnit.h"
#include <Eigen/Dense>
-#define CALIBRATE_ON 0
-
namespace armarx
{
class OrientedTactileSensorUnitPropertyDefinitions:
@@ -39,11 +36,27 @@ namespace armarx
"65524 3 12 65534 65534 1 1208 119 58726 1000 943 ",
"Sensor Register Data to calibrate the sensor");
+ defineOptionalProperty<std::string>(
+ "SamplesRotation",
+ "20",
+ "number of orientation values to differentiate");
+
+ defineOptionalProperty<std::string>(
+ "SamplesPressure",
+ "10",
+ "number of pressure values to differentiate");
+
+ defineOptionalProperty<std::string>(
+ "SamplesAcceleration",
+ "20",
+ "number of pressure values to differentiate");
+
defineOptionalProperty<bool>(
"calibrateSensor",
"false"
"Set true to calibrate the sensor and get calibration data and false to use existent calibration data");
}
+
};
/**
@@ -114,13 +127,13 @@ namespace armarx
std::vector<RotationRate> samplesRotation;
std::vector<PressureRate> samplesPressure;
- std::vector<AccelerationRate> samplesRotationRate;
+ std::vector<AccelerationRate> samplesAcceleration;
int maxSamplesRotation;
int sampleIndexRotation;
int maxSamplesPressure;
int sampleIndexPressure;
- int maxSamplesRotationRate;
- int sampleIndexRotationRate;
+ int maxSamplesAcceleration;
+ int sampleIndexAcceleration;
};
}
#endif // SENSORPACKAGEUNIT_H
diff --git a/source/RobotAPI/interface/units/OrientedTactileSensorUnit.ice b/source/RobotAPI/interface/units/OrientedTactileSensorUnit.ice
index 148e12f39..4f5eb63dc 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 pressureRate, float rotationRate, TimestampBase timestamp);
+ void reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float pressureRate, float rotationRate, float accelerationRate, TimestampBase timestamp);
};
--
GitLab