From ddb45457cf14160f3cb2cf5d171e694fcdadfcb6 Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Wed, 8 Mar 2017 16:48:47 +0100
Subject: [PATCH] added OptoForce daq calibration
---
.../RobotAPI/sensors/OptoForceCalibration.xml | 11 +
.../units/OptoForceUnitObserver.cpp | 19 +-
.../drivers/OptoForceUnit/OptoForceUnit.cpp | 215 ++++++++++--------
.../drivers/OptoForceUnit/OptoForceUnit.h | 23 +-
4 files changed, 166 insertions(+), 102 deletions(-)
create mode 100644 data/RobotAPI/sensors/OptoForceCalibration.xml
diff --git a/data/RobotAPI/sensors/OptoForceCalibration.xml b/data/RobotAPI/sensors/OptoForceCalibration.xml
new file mode 100644
index 000000000..b863b0de8
--- /dev/null
+++ b/data/RobotAPI/sensors/OptoForceCalibration.xml
@@ -0,0 +1,11 @@
+<Calibration>
+ <Daq serialNumber="KIT0A003">
+ <Sensor nominalCapacity="10" counts_at_nc="3870" name="DSE0A102"/>
+ <Sensor nominalCapacity="10" counts_at_nc="4001" name="DSE0A103"/>
+ <Sensor nominalCapacity="10" counts_at_nc="4021" name="DSE0A104"/>
+ <Sensor nominalCapacity="10" counts_at_nc="4048" name="DSE0A108"/>
+ </Daq>
+ <Daq serialNumber="DSE0A114">
+ <Sensor nominalCapacity="10" counts_at_nc="4048" name="DSE0A114" />
+ </Daq>
+</Calibration>
diff --git a/source/RobotAPI/components/units/OptoForceUnitObserver.cpp b/source/RobotAPI/components/units/OptoForceUnitObserver.cpp
index f70392b30..0b43e68d9 100644
--- a/source/RobotAPI/components/units/OptoForceUnitObserver.cpp
+++ b/source/RobotAPI/components/units/OptoForceUnitObserver.cpp
@@ -69,19 +69,20 @@ void OptoForceUnitObserver::reportSensorValues(const std::string& device, const
ScopedLock lock(dataMutex);
TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
- if (!existsChannel(device))
+ if (!existsChannel(name))
{
- offerChannel(device, "Force data");
+ offerChannel(name, "Force data");
}
- offerOrUpdateDataField(device, "name", Variant(name), "Name of the sensor");
- offerOrUpdateDataField(device, "fx", Variant(fx), "Force x");
- offerOrUpdateDataField(device, "fy", Variant(fy), "Force y");
- offerOrUpdateDataField(device, "fz", Variant(fz), "Force z");
- offerOrUpdateDataField(device, "timestamp", timestampPtr, "Timestamp");
- offerOrUpdateDataField(device, "force", Vector3(fx, fy, fz), "Force");
+ offerOrUpdateDataField(name, "name", Variant(name), "Name of the sensor");
+ offerOrUpdateDataField(name, "device", Variant(device), "Device name");
+ offerOrUpdateDataField(name, "fx", Variant(fx), "Force x");
+ offerOrUpdateDataField(name, "fy", Variant(fy), "Force y");
+ offerOrUpdateDataField(name, "fz", Variant(fz), "Force z");
+ offerOrUpdateDataField(name, "timestamp", timestampPtr, "Timestamp");
+ offerOrUpdateDataField(name, "force", Vector3(fx, fy, fz), "Force");
- updateChannel(device);
+ updateChannel(name);
}
/*void OptoForceUnitObserver::reportSensorValues(const std::string& device, const std::string& name, const IMUData& values, const TimestampBasePtr& timestamp, const Ice::Current& c)
diff --git a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp
index a9f44abd1..9fdfeaf16 100644
--- a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp
+++ b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp
@@ -25,144 +25,133 @@
#include <ArmarXCore/observers/variant/TimestampVariant.h>
+
using namespace armarx;
void OptoForceUnit::onInitComponent()
{
+ std::string calibrationFilePath = getProperty<std::string>("CalibrationFilePath").getValue();
+ if(!ArmarXDataPath::getAbsolutePath(calibrationFilePath, calibrationFilePath))
+ {
+ throw LocalException("Could not find calibration file '") << calibrationFilePath << "'";
+ }
+ ARMARX_INFO << "reading config " << calibrationFilePath;
+ RapidXmlReaderPtr reader = RapidXmlReader::FromFile(calibrationFilePath);
+ RapidXmlReaderNode rootNode = reader->getRoot("Calibration");
+
+ auto findDaqNode = [&](std::string serialNumber)
+ {
+ for(RapidXmlReaderNode daqNode : rootNode.nodes("Daq"))
+ {
+ if(daqNode.attribute_value("serialNumber") == serialNumber)
+ {
+ return daqNode;
+ }
+ }
+ return RapidXmlReaderNode::NullNode();
+ };
+
offeringTopic(getProperty<std::string>("OptoForceTopicName").getValue());
OPort* portlist = ports.listPorts(true);
ARMARX_INFO << "Found " << ports.getLastSize() << " Optoforce device(s).";
- int numPorts = ports.getLastSize();
- ARMARX_INFO << "number of ports: " << numPorts;
-
- OPort* portListNew = ports.listPorts(true);
- ARMARX_INFO << "Found " << ports.getLastSize() << " Optoforce device(s).";
+ portlist = ports.listPorts(true);
+ ARMARX_INFO << "Found " << ports.getLastSize() << " Optoforce device(s):";
for (int i = 0; i < ports.getLastSize(); i++)
{
- ARMARX_INFO << "device name: " << portListNew[i].name;
- daqList.push_back(OptoDAQ());
+ std::string deviceName(portlist[i].name);
+ std::string serialNumber(portlist[i].serialNumber);
+ ARMARX_INFO << "Found DAQ: deviceName='" << deviceName << "', serialNumber='" << serialNumber << "'";
+ RapidXmlReaderNode daqNode = findDaqNode(serialNumber);
+ if(!daqNode.is_valid())
+ {
+ throw LocalException("Could not find config node for device deviceName='") << deviceName << "', serialNumber='" << serialNumber << "'";
+ }
+ DaqWrapperPtr daqPtr(new DaqWrapper(deviceName, serialNumber, daqNode));
+ daqList.push_back(daqPtr);
}
ARMARX_INFO << "number of ports: " << ports.getLastSize();
- if (ports.getLastSize() > 0)
+ for (int i = 0; i < ports.getLastSize(); ++i)
{
- for (int i = 0; i < ports.getLastSize(); ++i)
- {
- ARMARX_INFO << "Opening port #" << i << " " << portListNew[i].name;
- //daqList.at(i).open(portListNew[i]);
- if (i == 0)
- {
- daq.open(portListNew[i]);
- ARMARX_INFO << "opened port 1";
- }
- if (daq.getVersion() == _95 || daq.getVersion() == _64)
- {
- ARMARX_IMPORTANT << "Wrong sensor type! (not 3D)";
- daq.close();
- return;
- }
- }
+ const DaqWrapperPtr& daq = daqList.at(i);
+ ARMARX_IMPORTANT << "Opening port #" << i << " " << portlist[i].name;
+ daq->daq.open(portlist[i]);
+ daq->printInfo();
+ daq->checkSensorCount();
}
readTask = new RunningTask<OptoForceUnit>(this, &OptoForceUnit::run, "OptoForceUnit");
- readTask->start();
+
}
void OptoForceUnit::onConnectComponent()
{
topicPrx = getTopic<OptoForceUnitListenerPrx>(getProperty<std::string>("OptoForceTopicName").getValue());
+ readTask->start();
}
void OptoForceUnit::run()
{
ARMARX_IMPORTANT << "run";
- SensorConfig config = daq.getConfig();
-
- int state = config.getState();
- int speed = config.getSpeed();
- int filter = config.getFilter();
- int mode = config.getMode();
-
- ARMARX_IMPORTANT << "state: " << state;
- ARMARX_IMPORTANT << "speed: " << speed;
- ARMARX_IMPORTANT << "filter: " << filter;
- ARMARX_IMPORTANT << "mode: " << mode;
OptoForceUnitListenerPrx batchPrx = topicPrx->ice_batchOneway();
+ ARMARX_IMPORTANT << "got batch Proxy";
while(readTask->isRunning())
{
- OptoPackage* pa = 0;
- //OptoPackage* pa2 = 0;
+ bool flushNeeded = false;
- int size = daq.readAll(pa, false);
- if (size <= 0)
+ for(const DaqWrapperPtr& daqPtr : daqList)
{
- usleep(1000); // 1ms
- continue;
- }
- //ARMARX_IMPORTANT << "size: " << size;
- //ARMARX_IMPORTANT << "daq.getSize(): " << daq.getSize();
- // necessary so that there are no lines without values
- if (daq.getSize() > 0)
- {
- //optoForceOutputFile << "getsize: " << daq.getSize() << "\n";
- //optoForceOutputFile << "getBytesperRead: " << daq.getBytesPerRead() << "\n";
-
- //optoForceOutputFile << "getsize2: " << daq2.getSize() << "\n";
- //optoForceOutputFile << "getBytesperRead2: " << daq2.getBytesPerRead() << "\n";
-
- //optoForceOutputFile << IceUtil::Time::now().toDateTime() << " OptoForce: ";
- // get values of first DAQ - 4 OptoForce sensors connected
- IceUtil::Time now = IceUtil::Time::now();
- TimestampVariantPtr nowTimestamp = new TimestampVariant(now);
-
-
- for (int k = 0; k < daq.getSensorSize(); k++)
+ OptoPackage* pa = 0;
+ int size = daqPtr->daq.readAll(pa, false);
+ if (size == 0)
{
- //ARMARX_INFO << "Sensor " << k;
-
- for (int i = 0; i < size; i++)
- {
- float x = pa[k * size + i].x;
- float y = pa[k * size + i].y;
- float z = pa[k * size + i].z;
-
- //ARMARX_INFO << "x: " << pa[k * size + i].x << " y: " << pa[k * size + i].y << " z: " << pa[k * size + i].z << " s1: " << pa[k * size + i].s1 << " s2: " << pa[k * size + i].s2 << " s3: " << pa[k * size + i].s3 << " s4: " << pa[k * size + i].s4 << " TEMP: " << pa[k * size + i].temp << flush;
- //optoForceOutputFile << "x: " << pa[k * size + i].x << " y: " << pa[k * size + i].y << " z: " << pa[k * size + i].z << " ";
- batchPrx->reportSensorValues("sensor" + std::to_string(k), "sensor" + std::to_string(k), x, y, z, nowTimestamp);
- }
+ // size == 0: no new data for daq
+ continue;
}
-
- batchPrx->ice_flushBatchRequests();
-
- /*// get values of second DAQ - 3 OptoForce sensors connected
- //needs to be (daq2.getSensorSize()-1) to read out only 3 ports
- for (int k = 0; k < (daq2.getSensorSize() - 1) ; k++)
+ if(size < 0)
{
- ARMARX_INFO << "Sensor " << k;
-
+ // size == -1: buffer full
+ ARMARX_WARNING << "buffer full of daq " << daqPtr->deviceName;
+ }
+ if (daqPtr->daq.getSize() > 0)
+ {
+ IceUtil::Time now = IceUtil::Time::now();
+ TimestampVariantPtr nowTimestamp = new TimestampVariant(now);
- for (int i = 0; i < size2; i++)
+ for (int i = 0; i < daqPtr->daq.getSensorSize(); i++)
{
- ARMARX_INFO << "x: " << pa2[k * size2 + i].x << " y: " << pa2[k * size2 + i].y << " z: " << pa2[k * size2 + i].z << " s1: " << pa2[k * size2 + i].s1 << " s2: " << pa2[k * size2 + i].s2 << " s3: " << pa2[k * size2 + i].s3 << " s4: " << pa2[k * size2 + i].s4 << " TEMP: " << pa2[k * size2 + i].temp << flush;
- optoForceOutputFile << "x: " << pa2[k * size2 + i].x << " y: " << pa2[k * size2 + i].y << " z: " << pa2[k * size2 + i].z << " ";
-
+ for (int n = 0; n < size; n++)
+ {
+ float countsPerN = daqPtr->countsPerN.at(i);
+ float x = pa[i * size + n].x / countsPerN;
+ float y = pa[i * size + n].y / countsPerN;
+ float z = pa[i * size + n].z / countsPerN;
+
+ batchPrx->reportSensorValues(daqPtr->deviceName + ":" + std::to_string(i), daqPtr->sensorNames.at(i), x, y, z, nowTimestamp);
+ flushNeeded = true;
+ }
}
- }*/
- // buffer needs to be deleted
- delete[] pa;
- //delete[] pa2;
- //optoForceOutputFile << "\n";
- //optoForceOutputFile.flush();
+
+ delete[] pa;
+ }
+ }
+ if(flushNeeded)
+ {
+ batchPrx->ice_flushBatchRequests();
+ }
+ else
+ {
+ usleep(1000); // 1ms
}
}
}
@@ -185,3 +174,47 @@ armarx::PropertyDefinitionsPtr OptoForceUnit::createPropertyDefinitions()
getConfigIdentifier()));
}
+
+
+OptoForceUnit::DaqWrapper::DaqWrapper(const std::string& deviceName, const std::string& serialNumber, const RapidXmlReaderNode& daqNode)
+ : deviceName(deviceName), serialNumber(serialNumber)
+{
+ int i = 0;
+ for(RapidXmlReaderNode sensorNode : daqNode.nodes("Sensor"))
+ {
+ float counts_at_nc = sensorNode.attribute_as_float("counts_at_nc");
+ float nominalCapacity = sensorNode.attribute_as_float("nominalCapacity");
+ std::string sensorName = sensorNode.attribute_value_or_default("name", serialNumber + "-" + std::to_string(i));
+ countsPerN.push_back(counts_at_nc / nominalCapacity);
+ sensorNames.push_back(sensorName);
+ i++;
+ }
+}
+
+void OptoForceUnit::DaqWrapper::printInfo()
+{
+ SensorConfig config = daq.getConfig();
+ int state = config.getState();
+ int speed = config.getSpeed();
+ int filter = config.getFilter();
+ int mode = config.getMode();
+
+ ARMARX_IMPORTANT_S << "device: " << deviceName << " serialNumber:" << serialNumber;
+ ARMARX_IMPORTANT_S << "state: " << state;
+ ARMARX_IMPORTANT_S << "speed: " << speed;
+ ARMARX_IMPORTANT_S << "filter: " << filter;
+ ARMARX_IMPORTANT_S << "mode: " << mode;
+ ARMARX_IMPORTANT_S << "getBytesPerRead: " << daq.getBytesPerRead();
+ ARMARX_IMPORTANT_S << "getSensorSize: " << daq.getSensorSize();
+ ARMARX_IMPORTANT_S << "getSize: " << daq.getSize();
+ ARMARX_IMPORTANT_S << "getVersion: " << daq.getVersion();
+}
+
+void OptoForceUnit::DaqWrapper::checkSensorCount()
+{
+ if((int)countsPerN.size() != daq.getSensorSize())
+ {
+ throw LocalException("Sensor count mismatch. Configured: ") << ((int)countsPerN.size()) << " found: " << daq.getSensorSize();
+ }
+ ARMARX_INFO_S << "Configured: " << ((int)countsPerN.size()) << " found: " << daq.getSensorSize() << " sensors";
+}
diff --git a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h
index 695d9c048..50c7fccdd 100644
--- a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h
+++ b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h
@@ -28,6 +28,7 @@
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <RobotAPI/interface/units/OptoForceUnit.h>
#include <OptoForce/omd/opto.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
namespace armarx
{
@@ -44,6 +45,7 @@ namespace armarx
{
//defineRequiredProperty<std::string>("PropertyName", "Description");
defineOptionalProperty<std::string>("OptoForceTopicName", "OptoForceValues", "Name of the OptoForce Topic");
+ defineOptionalProperty<std::string>("CalibrationFilePath", "RobotAPI/sensors/OptoForceCalibration.xml", "Path of the Calibration File");
}
};
@@ -61,6 +63,23 @@ namespace armarx
class OptoForceUnit :
virtual public armarx::Component
{
+ private:
+ class DaqWrapper
+ {
+ public:
+ DaqWrapper(const std::string& deviceName, const std::string& serialNumber, const RapidXmlReaderNode& daqNode);
+
+ OptoDAQ daq;
+ std::string deviceName;
+ std::string serialNumber;
+ std::vector<float> countsPerN;
+ std::vector<std::string> sensorNames;
+
+ void printInfo();
+ void checkSensorCount();
+ };
+ typedef boost::shared_ptr<DaqWrapper> DaqWrapperPtr;
+
public:
/**
* @see armarx::ManagedIceObject::getDefaultName()
@@ -102,8 +121,8 @@ namespace armarx
OptoForceUnitListenerPrx topicPrx;
OptoPorts ports;
- OptoDAQ daq;
- std::vector<OptoDAQ> daqList;
+ //OptoDAQ daq;
+ std::vector<DaqWrapperPtr> daqList;
RunningTask<OptoForceUnit>::pointer_type readTask;
};
--
GitLab