GenericTopicSubscriber.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    ArmarX
* @author     Mirko Waechter( mirko.waechter at kit dot edu)
* @date       2016
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
#include "GenericTopicSubscriber.h"
 
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include "TopicRecorder.h"
 
namespace armarx
{
 
    GenericTopicSubscriber::GenericTopicSubscriber(TopicRecorderComponentPtr recorder,
                                                   const std::string& topicName,
                                                   IceUtil::Time startTimestamp,
                                                   float maxFrequency) :
        topicName(topicName),
        recorder(recorder),
        startTimestamp(startTimestamp),
        maxFrequency(maxFrequency)
    {
    }
 
    void
    GenericTopicSubscriber::getData(std::queue<TopicUtil::TopicData>& data)
    {
        std::unique_lock lock(queueMutex);
        data.swap(dataQueue);
    }
 
    bool
    GenericTopicSubscriber::ice_invoke(const std::vector<Ice::Byte>& inParams,
                                       std::vector<Ice::Byte>& outParams,
                                       const Ice::Current& current)
    {
        std::unique_lock lock(queueMutex);
        auto now = TimeUtil::GetTime();
 
        if (!checkTimestamp(current.operation, now))
        {
            return true;
        }
 
 
        dataQueue.emplace(topicName, now - startTimestamp, current.operation, inParams);
        if (recorder)
        {
            recorder->wakeUp();
        }
        return true;
    }
 
    bool
    GenericTopicSubscriber::checkTimestamp(const std::string& operationName,
                                           const IceUtil::Time& timestamp)
    {
        if (maxFrequency > 0)
        {
            auto it = functionCallTimestamps.find(operationName);
            if (it != functionCallTimestamps.end())
            {
                if ((timestamp - it->second).toSecondsDouble() < 1.0 / maxFrequency)
                {
                    return false;
                }
                else
                {
                    it->second = timestamp;
                }
            }
            else
            {
                functionCallTimestamps[operationName] = timestamp;
            }
        }
        return true;
    }
 
} // namespace armarx