OptoForceUnit.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotAPI::ArmarXObjects::OptoForceUnit
 * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "OptoForceUnit.h"
 
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
 
using namespace armarx;
 
ARMARX_REGISTER_COMPONENT_EXECUTABLE(armarx::OptoForceUnit, "OptoForceUnit");
 
OptoForceUnit::OptoForceUnit() : isRecording(false), stopRecordingFlag(false)
{
}
 
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).";
 
 
    portlist = ports.listPorts(true);
    ARMARX_INFO << "Found " << ports.getLastSize() << " Optoforce device(s):";
 
    for (int i = 0; i < ports.getLastSize(); i++)
    {
        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();
 
    for (int i = 0; i < ports.getLastSize(); ++i)
    {
        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");
}
 
void
OptoForceUnit::onConnectComponent()
{
    topicPrx = getTopic<OptoForceUnitListenerPrx>(
        getProperty<std::string>("OptoForceTopicName").getValue());
    readTask->start();
}
 
void
OptoForceUnit::run()
{
    ARMARX_IMPORTANT << "run";
 
 
    OptoForceUnitListenerPrx batchPrx = topicPrx->ice_batchOneway();
    ARMARX_IMPORTANT << "got batch Proxy";
 
    while (readTask->isRunning())
    {
        bool flushNeeded = false;
 
        for (const DaqWrapperPtr& daqPtr : daqList)
        {
            OptoPackage* pa = 0;
            int size = daqPtr->daq.readAll(pa, false);
            if (size == 0)
            {
                // size == 0: no new data for daq
                continue;
            }
            if (size < 0)
            {
                // 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);
 
                int sensorCount = daqPtr->daq.getSensorSize();
 
                std::vector<std::array<float, 3>> data;
                data.resize(sensorCount);
 
                for (int i = 0; i < sensorCount; i++)
                {
                    if (!daqPtr->enableFlags.at(i))
                    {
                        continue;
                    }
                    for (int n = 0; n < size; n++)
                    {
                        float countsPerN = daqPtr->countsPerN.at(i);
                        float x = pa[i * size + n].x / countsPerN - daqPtr->offsets.at(i).at(0);
                        float y = pa[i * size + n].y / countsPerN - daqPtr->offsets.at(i).at(1);
                        float z = pa[i * size + n].z / countsPerN - daqPtr->offsets.at(i).at(2);
 
                        batchPrx->reportSensorValues(daqPtr->deviceName + ":" + to_string(i),
                                                     daqPtr->sensorNames.at(i),
                                                     x,
                                                     y,
                                                     z,
                                                     nowTimestamp);
                        flushNeeded = true;
                        data.at(i) = {x, y, z};
                    }
                }
                delete[] pa;
 
                if (isRecording)
                {
                    recordingFile << "{\"timestamp\":\"" << now.toDateTime() << "\",";
                    recordingFile << "\"daq\":\"" << daqPtr->serialNumber << "\",\"data\":[";
                    bool first = true;
                    for (int i = 0; i < sensorCount; i++)
                    {
                        if (!daqPtr->enableFlags.at(i))
                        {
                            continue;
                        }
                        if (!first)
                        {
                            recordingFile << ",";
                        }
                        first = false;
                        recordingFile << "[" << data.at(i).at(0) << "," << data.at(i).at(1) << ","
                                      << data.at(i).at(2) << "]";
                    }
                    recordingFile << "]}\n";
                    recordingFile.flush();
                }
            }
        }
        if (isRecording && stopRecordingFlag)
        {
            recordingFile.close();
            isRecording = false;
        }
        stopRecordingFlag = false;
        if (flushNeeded)
        {
            batchPrx->ice_flushBatchRequests();
        }
        else
        {
            usleep(1000); // 1ms
        }
    }
}
 
void
OptoForceUnit::onDisconnectComponent()
{
}
 
void
OptoForceUnit::onExitComponent()
{
    readTask->stop();
}
 
armarx::PropertyDefinitionsPtr
OptoForceUnit::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new OptoForceUnitPropertyDefinitions(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 + "-" + to_string(i));
        countsPerN.push_back(counts_at_nc / nominalCapacity);
        sensorNames.push_back(sensorName);
        enableFlags.push_back(
            sensorNode.attribute_as_optional_bool("enabled", "true", "false", true));
        offsets.push_back({sensorNode.attribute_as_float("offsetX"),
                           sensorNode.attribute_as_float("offsetY"),
                           sensorNode.attribute_as_float("offsetZ")});
        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";
}
 
void
armarx::OptoForceUnit::startRecording(const std::string& filepath, const Ice::Current&)
{
    ARMARX_IMPORTANT << "start recording: " << filepath;
    recordingFile.open(filepath);
    isRecording = true;
}
 
void
armarx::OptoForceUnit::stopRecording(const Ice::Current&)
{
    stopRecordingFlag = true;
}