WeissHapticSensor.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "WeissHapticSensor.h"
 
#include <boost/format.hpp>
#include <boost/regex.hpp>
 
#include "TransmissionException.h"
 
using namespace armarx;
 
WeissHapticSensor::WeissHapticSensor(std::string device, int minimumReportIntervalMs) :
    device(device),
    connected(false),
    setDeviceTagScheduled(false),
    minimumReportIntervalMs(minimumReportIntervalMs)
{
    sensorTask =
        new RunningTask<WeissHapticSensor>(this, &WeissHapticSensor::frameAcquisitionTaskLoop);
    boost::smatch match;
    boost::regex_search(device, match, boost::regex("\\w+$"));
    this->deviceFileName = match[0];
}
 
void
WeissHapticSensor::connect()
{
 
    ARMARX_INFO << "Open Serial" << std::endl;
    this->interface.reset(new SerialInterface(device.c_str(), 115200));
    //interface->startLogging(deviceFileName + ".transmission.log");
    interface->open();
    //cout << *interface << endl;
    this->sensor.reset(new TactileSensor(interface));
 
    jsWriter.reset(new TextWriter(deviceFileName + ".js"));
 
    //cout << "Stop Periodic Frame Acquisition" << endl;
    sensor->stopPeriodicFrameAcquisition();
 
    //string sensorType = sensor->getSensorType();
    //cout << boost::format("Sensor Type = %1%") % sensorType << endl;
 
    //tac_system_information_t si = sensor->getSystemInformation();
    //TactileSensor::printSystemInformation(si);
 
    //sensor->setDeviceTag("Test Tag");
    std::string tag;
 
    if (sensor->tryGetDeviceTag(tag))
    {
        ARMARX_INFO << "[" << device << "] Got Device Tag: " << tag;
        this->tag = tag;
    }
    else
    {
        ARMARX_WARNING << "[" << device << "] No device tag present using device name instead";
        this->tag = deviceFileName;
    }
 
    //cout << boost::format("Sensor Tag = %1%") % tag << endl;
 
    jsWriter->writeLine(str(boost::format("%s = {};") % deviceFileName));
    jsWriter->writeLine(str(boost::format("sensors.push(%s);") % deviceFileName));
    jsWriter->writeLine(str(boost::format("%s.device = \"%s\";") % deviceFileName % device));
    jsWriter->writeLine(str(boost::format("%s.tag = \"%s\";") % deviceFileName % tag));
    jsWriter->writeLine(str(boost::format("%s.data = [];") % deviceFileName));
 
    //cout << "Tare Sensor Matrix" << endl;
    //sensor->tareSensorMatrix(1);
 
    //cout << "Get Matrix Information" << endl;
    this->mi = sensor->getMatrixInformation();
    TactileSensor::printMatrixInfo(&mi);
 
    sensor->setAquisitionWindow(1, 1, mi.res_x, mi.res_y);
 
    sensor->setFrontEndGain(255);
    ARMARX_LOG << "[" << device << "] Front end gain set to " << (int)sensor->getFrontEndGain();
 
    sensor->setThreshold(0);
    ARMARX_LOG << "[" << device << "] threshold set to " << (int)sensor->getThreshold();
 
    connected = true;
    ARMARX_LOG << device << ": Connect done, Interface=" << sensor->getInterfaceInfo();
}
 
void
WeissHapticSensor::disconnect()
{
    connected = false;
    sensorTask->stop(true);
}
 
void
WeissHapticSensor::setListenerPrx(HapticUnitListenerPrx listenerPrx)
{
    this->listenerPrx = listenerPrx;
}
 
void
WeissHapticSensor::startSampling()
{
    ARMARX_LOG << device << ": startSampling" << std::endl;
    sensorTask->start();
}
 
std::string
WeissHapticSensor::getDeviceName()
{
    return device;
}
 
void
WeissHapticSensor::scheduleSetDeviceTag(std::string tag)
{
    std::unique_lock lock(mutex);
    setDeviceTagValue = tag;
    setDeviceTagScheduled = true;
}
 
void
WeissHapticSensor::frameAcquisitionTaskLoop()
{
    ARMARX_LOG << device << ": readAndReportSensorValues";
    //bool periodic = false;
    ARMARX_LOG << device << ": Interface Info: " << sensor->getInterfaceInfo();
 
    ARMARX_LOG << device << this << ": startPeriodicFrameAcquisition";
    sensor->startPeriodicFrameAcquisition(0);
 
    IceUtil::Time lastFrameTime = IceUtil::Time::now();
 
    math::SlidingWindowVectorMedian slidingMedian(
        mi.res_x * mi.res_y, 21); // inter sample dely ~= 3,7ms, 11 samples ~== 40ms delay
 
 
    while (!sensorTask->isStopped())
    {
        //ARMARX_INFO << deactivateSpam(1) << this << ": receicePeriodicFrame";
 
        try
        {
            //long start = TimestampVariant::nowLong();
            PeriodicFrameData data = sensor->receicePeriodicFrame();
            //long end = TimestampVariant::nowLong();
            //cout << end - start << endl;
 
            std::vector<float> sensorValues;
 
            for (int i = 0; i < mi.res_x * mi.res_y; i++)
            {
                sensorValues.push_back(data.data->at(i));
            }
 
            slidingMedian.addEntry(sensorValues);
 
            MatrixFloatPtr matrix = new MatrixFloat(mi.res_y, mi.res_x);
            std::vector<float> filteredSensorValues = slidingMedian.getMedian();
 
            for (int y = 0; y < mi.res_y; y++)
            {
                for (int x = 0; x < mi.res_x; x++)
                {
                    (*matrix)(y, x) = filteredSensorValues.at(y * mi.res_x + x);
                }
            }
 
 
            /*
            MatrixFloatPtr matrix = new MatrixFloat(mi.res_y, mi.res_x);
            for (int y = 0; y < mi.res_y; y++)
            {
                for (int x = 0; x < mi.res_x; x++)
                {
                    short val = (*data.data)[y * mi.res_x + x];
                    (*matrix)(y, x) = val;
                }
            }
            */
 
            IceUtil::Time now = IceUtil::Time::now();
 
            TimestampVariantPtr nowTimestamp = new TimestampVariant(now);
            writeMatrixToJs(matrix, nowTimestamp);
 
            IceUtil::Time interval = now - lastFrameTime;
 
            if (interval.toMilliSeconds() >= minimumReportIntervalMs)
            {
                listenerPrx->reportSensorValues(device, tag, matrix, nowTimestamp);
                lastFrameTime = now;
            }
        }
        catch (ChecksumErrorException&)
        {
            ARMARX_WARNING << "Caught ChecksumErrorException on " << device << ", skipping frame";
        }
 
        if (setDeviceTagScheduled)
        {
            std::unique_lock lock(mutex);
            setDeviceTagScheduled = false;
 
            ARMARX_INFO << "[" << device
                        << "] Stopping periodic frame aquisition to set new device tag";
            sensor->stopPeriodicFrameAcquisition();
 
            ARMARX_IMPORTANT << "[" << device << "] Setting new device tag '" << setDeviceTagValue
                             << "'";
            sensor->setDeviceTag(setDeviceTagValue);
            this->tag = setDeviceTagValue;
 
            ARMARX_INFO << "[" << device << "] Starting periodic frame aquisition";
            sensor->startPeriodicFrameAcquisition(0);
        }
    }
 
    ARMARX_LOG << device << ": stopPeriodicFrameAcquisition";
    sensor->stopPeriodicFrameAcquisition();
}
 
void
WeissHapticSensor::writeMatrixToJs(MatrixFloatPtr matrix, TimestampVariantPtr timestamp)
{
    //std::cout << "writeMatrixToJs" << std::endl;
    if (jsWriter != NULL)
    {
        jsWriter->writeLine(str(boost::format("%s.data.push([%i, %s]);") % deviceFileName %
                                timestamp->getTimestamp() % matrix->toJsonRowMajor()));
    }
}