HapticObserver.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 "HapticObserver.h"
 
#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
#include <RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h>
 
#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
#include <Eigen/Dense>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
namespace armarx
{
    HapticObserver::HapticObserver()
    {
        statisticsTask = new PeriodicTask<HapticObserver>(this, &HapticObserver::updateStatistics, 10, false);
    }
 
    void HapticObserver::setTopicName(std::string topicName)
    {
        this->topicName = topicName;
    }
 
    void HapticObserver::onInitObserver()
    {
        if (topicName.empty())
        {
            usingTopic(getProperty<std::string>("HapticTopicName").getValue());
        }
        else
        {
            usingTopic(topicName);
        }
 
        // register all checks
        offerConditionCheck("updated", new ConditionCheckUpdated());
        offerConditionCheck("larger", new ConditionCheckLarger());
        offerConditionCheck("equals", new ConditionCheckEquals());
        offerConditionCheck("smaller", new ConditionCheckSmaller());
    }
 
    void HapticObserver::onConnectObserver()
    {
        statisticsTask->start();
    }
 
    void HapticObserver::onExitObserver()
    {
        statisticsTask->stop();
    }
 
    void HapticObserver::reportSensorValues(const std::string& device, const std::string& name, const armarx::MatrixFloatBasePtr& values, const armarx::TimestampBasePtr& timestamp, const Ice::Current&)
    {
        std::unique_lock lock(dataMutex);
 
        MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(values);
        if (matrix->cols == 0)
        {
            // Empty matrix received, silently ignore
            return;
        }
 
        TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
        Eigen::MatrixXf eigenMatrix = matrix->toEigen();
        float max = eigenMatrix.maxCoeff();
        float mean = eigenMatrix.mean();
        std::string channelName = name;
        Eigen::MatrixXf M = matrix->toEigen();
 
        if (!existsChannel(channelName))
        {
            offerChannel(channelName, "Haptic data");
            offerDataFieldWithDefault(channelName, "device", Variant(device), "Device of the tactile sensor");
            offerDataFieldWithDefault(channelName, "name", Variant(name), "Name of the tactile sensor");
            offerDataFieldWithDefault(channelName, "matrix", matrix, "Raw tactile matrix data");
            offerDataFieldWithDefault(channelName, "max", Variant(max), "Maximum value");
            offerDataFieldWithDefault(channelName, "mean", Variant(mean), "Mean value");
            offerDataFieldWithDefault(channelName, "timestamp", timestampPtr, "Timestamp");
            offerDataFieldWithDefault(channelName, "rate", Variant(0.0f), "Sample rate");
 
            for (int i = 0; i < M.rows(); i++)
            {
                for (int j = 0; j < M.cols(); j++)
                {
                    std::stringstream s;
                    s << "entry_" << i << "," << j;
                    offerDataFieldWithDefault(channelName, s.str(), Variant(M(i, j)), "Individual matrix entry");
                }
            }
 
            ARMARX_INFO << "Offering new channel: " << channelName;
        }
        else
        {
            setDataField(channelName, "device", Variant(device));
            setDataField(channelName, "name", Variant(name));
            setDataField(channelName, "matrix", matrix);
            setDataField(channelName, "max", Variant(max));
            setDataField(channelName, "mean", Variant(mean));
            setDataField(channelName, "timestamp", timestampPtr);
 
            for (int i = 0; i < M.rows(); i++)
            {
                for (int j = 0; j < M.cols(); j++)
                {
                    std::stringstream s;
                    s << "entry_" << i << "," << j;
                    setDataField(channelName, s.str(), Variant(M(i, j)));
                }
            }
 
        }
 
        /*if(statistics.count(device) > 0)
        {
            statistics.at(device).add(timestamp->timestamp);
            HapticSampleStatistics stats = statistics.at(device);
            long avg = stats.average();
            float rate = avg == 0 ? 0 : 1000000.0f / (float)avg;
            setDataField(device, "rate", Variant(rate));
        }
        else
        {
            statistics.insert(std::map<std::string,HapticSampleStatistics>::value_type(device, HapticSampleStatistics(100, timestamp->timestamp)));
        }*/
 
        updateChannel(channelName);
    }
 
    PropertyDefinitionsPtr HapticObserver::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(new HapticObserverPropertyDefinitions(getConfigIdentifier()));
    }
 
    void HapticObserver::updateStatistics()
    {
        /*std::unique_lock lock(dataMutex);
        //ARMARX_LOG << "updateStatistics";
        long now = TimestampVariant::nowLong();
        for (std::map<std::string, HapticSampleStatistics>::iterator it = statistics.begin(); it != statistics.end(); ++it)
        {
            HapticSampleStatistics stats = it->second;
            std::string device = it->first;
            long avg = stats.average(now);
            float rate = avg == 0 ? 0 : 1000000.0f / (float)avg;
            setDataField(device, "rate", Variant(rate));
            updateChannel(device);
        }*/
    }
}