added OptoForce daq calibration

data/RobotAPI/sensors/OptoForceCalibration.xml

+<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>

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)

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";
+}

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;
 
     };